diff --git a/robotnik_gazebo_ignition/hooks/robotnik_gazebo_ignition.sh.in b/robotnik_gazebo_ignition/hooks/robotnik_gazebo_ignition.sh.in index 408d17e..4d4ea24 100644 --- a/robotnik_gazebo_ignition/hooks/robotnik_gazebo_ignition.sh.in +++ b/robotnik_gazebo_ignition/hooks/robotnik_gazebo_ignition.sh.in @@ -28,12 +28,23 @@ for _prefix in $(printf %s "$_prefixes" | tr ':' ' '); do for _pkg in $_GZ_PKGS; do _p="$_prefix/share/$_pkg" if [ -d "$_p" ]; then - _prepend_unique_value GZ_SIM_RESOURCE_PATH "$(_get_parent_abs_path "$_p")" - _prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$(_get_parent_abs_path "$_p")" - _prepend_unique_value GAZEBO_RESOURCE_PATH "$(_get_parent_abs_path "$_p")" + _parent_path="$(_get_parent_abs_path "$_p")" + _model_path="${_parent_path}/$_pkg/models" + + # Always add the parent path + _prepend_unique_value GZ_SIM_RESOURCE_PATH "$_parent_path" + _prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$_parent_path" + _prepend_unique_value GAZEBO_RESOURCE_PATH "$_parent_path" + + # If 'models' subfolder exists, add that too + if [ -d "$_model_path" ]; then + _prepend_unique_value GZ_SIM_RESOURCE_PATH "$_model_path" + _prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$_model_path" + _prepend_unique_value GAZEBO_RESOURCE_PATH "$_model_path" + fi fi done done -unset _prefix _pkg _p _prefixes _self_prefix _hook_dir _GZ_PKGS +unset _prefix _pkg _p _prefixes _self_prefix _hook_dir _GZ_PKGS _parent_path _model_path unset -f _prepend_unique_value _get_parent_abs_path diff --git a/robotnik_gazebo_ignition/launch/spawn_robot.launch.py b/robotnik_gazebo_ignition/launch/spawn_robot.launch.py index 91ce359..7f795c6 100644 --- a/robotnik_gazebo_ignition/launch/spawn_robot.launch.py +++ b/robotnik_gazebo_ignition/launch/spawn_robot.launch.py @@ -175,7 +175,11 @@ def add_depth_camera(camera_name): (f"/{robot_id}/{camera_name}_camera_depth/depth/camera_info", f"/{robot_id}/{camera_name}_rgbd_camera/depth/camera_info", "sensor_msgs/msg/CameraInfo", "gz.msgs.CameraInfo", "GZ_TO_ROS"), (f"/{robot_id}/{camera_name}_camera_depth/depth/image_raw", f"/{robot_id}/{camera_name}_rgbd_camera/depth/image_raw", "sensor_msgs/msg/Image", "gz.msgs.Image", "GZ_TO_ROS"), ]) - + # Pose from gz simulator + def add_pose(): + bridge_raw.extend([ + ( f"/model/robot/pose", f"/model/robot/pose", "geometry_msgs/msg/PoseStamped", "gz.msgs.Pose", "GZ_TO_ROS"), + ]) add_camera("front") add_camera("rear") add_camera("top_ptz") @@ -183,6 +187,7 @@ def add_depth_camera(camera_name): add_laser("front") add_laser("rear") add_pointcloud("top") + add_pose() bridge_config = [{"ros_topic_name": ros, "gz_topic_name": gz, "ros_type_name": ros_type, "gz_type_name": gz_type, "direction": direction} for gz, ros, ros_type, gz_type, direction in bridge_raw] with tempfile.NamedTemporaryFile(mode='w', delete=False) as tmp: @@ -271,12 +276,13 @@ def get_ros2_control_yaml_path(params): executable="rviz2", namespace=params['robot_id'], arguments=[ - '-d', [FindPackageShare('robotnik_gazebo_ignition'), '/config/rviz_config.rviz'], + '-d', [params['rviz_config']], # Fixed frame - '-f', [params['robot_id'], '_odom'], + # '-f', [params['robot_id'], '_odom'], # Window name '-t', [params['robot_id'], ' - ', params['robot_model'], ' - RViz'], ], + parameters=[{'use_sim_time': True}], condition=IfCondition(params['run_rviz']) )) return ret @@ -288,11 +294,13 @@ def generate_launch_description(): ("robot", "Robot Model Name", "", "ROBOT"), ("robot_model", "Robot Variant or Type", LaunchConfiguration('robot'), "ROBOT_MODEL"), ("robot_xacro", "Path to Robot Xacro File", [FindPackageShare('robotnik_description'), '/robots/', LaunchConfiguration('robot'), '/', LaunchConfiguration('robot_model'), '.urdf.xacro'], "ROBOT_XACRO"), - ("x", "Initial X Coordinate", "0.0", "X"), - ("y", "Initial Y Coordinate", "0.0", "Y"), - ("z", "Initial Z Coordinate", "0.0", "Z"), + ("x", "Initial X Coordinate", "-11.058", "X"), + ("y", "Initial Y Coordinate", "-78.487", "Y"), + ("z", "Initial Z Coordinate", "0.05", "Z"), ("has_arm", "Enable Arm Controller", "False", "HAS_ARM"), ("run_rviz", "Run RViz", "True", "RUN_RVIZ"), + ("use_sim_time", "Use simulation time", "True", "USE_SIM_TIME"), + ("rviz_config", "RViz configuration file", [FindPackageShare('robotnik_gazebo_ignition'), '/config/rviz_config.rviz'], "CONFIG_RVIZ"), ] ld = LaunchDescription() diff --git a/robotnik_gazebo_ignition/models/gz_warehouse/thumbnails/1.png b/robotnik_gazebo_ignition/models/gz_warehouse/thumbnails/1.png new file mode 100644 index 0000000..d36b3f5 Binary files /dev/null and b/robotnik_gazebo_ignition/models/gz_warehouse/thumbnails/1.png differ diff --git a/robotnik_gazebo_ignition/models/warehouse_simple/thumbnails/thumbnail.png b/robotnik_gazebo_ignition/models/warehouse_simple/thumbnails/thumbnail.png new file mode 100644 index 0000000..21c3b6c Binary files /dev/null and b/robotnik_gazebo_ignition/models/warehouse_simple/thumbnails/thumbnail.png differ