Merge pull request #675 from JdeRobot/fix-drone-worlds-names

Fix drone worlds names

Author: javizqh

CustomRobots/CMakeLists.txt

@@ -113,7 +113,6 @@ install(
     autopark_harmonic/params
     # DRONE_ASSETS
     drone_assets/models
-    drone_assets/worlds
     drone_assets/bridges
     # GLOBAL_NAVIGATION
     taxi_navigator/models

CustomRobots/drone_assets/bridges/rescue_people.yaml

@@ -7,7 +7,7 @@
 
 # gz topic published by IMU plugin
 - ros_topic_name: "sensor_measurements/imu"
-  gz_topic_name: "/world/empty/model/drone0/model/imu/link/internal/sensor/imu/imu"
+  gz_topic_name: "/world/default/model/drone0/model/imu/link/internal/sensor/imu/imu"
   ros_type_name: "sensor_msgs/msg/Imu"
   gz_type_name: "gz.msgs.IMU"
   direction: GZ_TO_ROS
@@ -20,7 +20,7 @@
   direction: GZ_TO_ROS
 
 - ros_topic_name: "/tf"
-  gz_topic_name: "/world/empty/model/drone0/pose"
+  gz_topic_name: "/world/default/model/drone0/pose"
   ros_type_name: "tf2_msgs/msg/TFMessage"
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS
@@ -32,7 +32,7 @@
   direction: GZ_TO_ROS
 
 - ros_topic_name: "/tf"
-  gz_topic_name: "/world/empty/model/drone0/pose_static"
+  gz_topic_name: "/world/default/model/drone0/pose_static"
   ros_type_name: "tf2_msgs/msg/TFMessage"
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS
@@ -59,25 +59,25 @@
 
 # Payload plugin
 - ros_topic_name: "sensor_measurements/frontal_camera/image_raw"
-  gz_topic_name: "/world/empty/model/drone0/model/frontal_cam/link/hd_camera/sensor/camera/image"
+  gz_topic_name: "/world/default/model/drone0/model/frontal_cam/link/hd_camera/sensor/camera/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
 
 - ros_topic_name: "sensor_measurements/frontal_camera/camera_info"
-  gz_topic_name: "/world/empty/model/drone0/model/frontal_cam/link/hd_camera/sensor/camera/camera_info"
+  gz_topic_name: "/world/default/model/drone0/model/frontal_cam/link/hd_camera/sensor/camera/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
   direction: GZ_TO_ROS
 
 - ros_topic_name: "sensor_measurements/ventral_camera/image_raw"
-  gz_topic_name: "/world/empty/model/drone0/model/ventral_cam/link/hd_camera/sensor/camera/image"
+  gz_topic_name: "/world/default/model/drone0/model/ventral_cam/link/hd_camera/sensor/camera/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
 
 - ros_topic_name: "sensor_measurements/ventral_camera/camera_info"
-  gz_topic_name: "/world/empty/model/drone0/model/ventral_cam/link/hd_camera/sensor/camera/camera_info"
+  gz_topic_name: "/world/default/model/drone0/model/ventral_cam/link/hd_camera/sensor/camera/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
-  direction: GZ_TO_ROS
+  direction: GZ_TO_ROS

CustomRobots/drone_assets/worlds/rescue_people_harmonic.world

@@ -3,11 +3,22 @@
 
 import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, SetEnvironmentVariable
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+    SetEnvironmentVariable,
+)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from ament_index_python.packages import get_package_share_directory
@@ -19,20 +30,20 @@ def generate_launch_description():
     pkg_share_dir = get_package_share_directory("ur5_gripper_description")
     # Go up from install/ur5_gripper_description/share/ur5_gripper_description to install/
     workspace_dir = os.path.dirname(os.path.dirname(os.path.dirname(pkg_share_dir)))
-    
+
     # Prepare environment variables for Gazebo
-    gz_lib_path = os.path.join(workspace_dir, 'gz_ros2_control', 'lib')
-    ur_share = os.path.join(workspace_dir, 'ur5_gripper_description', 'share')
-    robotiq_share = os.path.join(workspace_dir, 'robotiq_description', 'share')
-    
+    gz_lib_path = os.path.join(workspace_dir, "gz_ros2_control", "lib")
+    ur_share = os.path.join(workspace_dir, "ur5_gripper_description", "share")
+    robotiq_share = os.path.join(workspace_dir, "robotiq_description", "share")
+
     gz_env = {
-        'GZ_SIM_RESOURCE_PATH': f"{ur_share}:{robotiq_share}:{os.environ.get('GZ_SIM_RESOURCE_PATH', '')}",
-        'GZ_SIM_SYSTEM_PLUGIN_PATH': f"{gz_lib_path}:{os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')}",
-        'LD_LIBRARY_PATH': f"{gz_lib_path}:{os.environ.get('LD_LIBRARY_PATH', '')}"
+        "GZ_SIM_RESOURCE_PATH": f"{ur_share}:{robotiq_share}:{os.environ.get('GZ_SIM_RESOURCE_PATH', '')}",
+        "GZ_SIM_SYSTEM_PLUGIN_PATH": f"{gz_lib_path}:{os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')}",
+        "LD_LIBRARY_PATH": f"{gz_lib_path}:{os.environ.get('LD_LIBRARY_PATH', '')}",
     }
     # Declare arguments
     declared_arguments = []
-    
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "ur_type",
@@ -41,23 +52,23 @@ def generate_launch_description():
             choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
         )
     )
-    
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "prefix",
             default_value='""',
             description="Prefix of the joint names",
         )
     )
-    
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "launch_rviz",
             default_value="false",
             description="Launch RViz?",
         )
     )
-    
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "gz_args",
@@ -76,15 +87,15 @@ def generate_launch_description():
     xacro_file = os.path.join(
         get_package_share_directory("ur5_gripper_description"),
         "urdf",
-        "ur5_robotiq85_gripper.urdf.xacro"
+        "ur5_robotiq85_gripper.urdf.xacro",
     )
-    
+
     controllers_file = os.path.join(
         get_package_share_directory("ur5_gripper_description"),
         "config",
-        "ur5_controllers.yaml"
+        "ur5_controllers.yaml",
     )
-    
+
     # Process xacro directly to avoid Command() stderr issues
     robot_description_content = xacro.process_file(
         xacro_file,
@@ -96,20 +107,17 @@ def generate_launch_description():
             "sim_gazebo": "false",
             "sim_gz": "true",
             "simulation_controllers": controllers_file,
-        }
+        },
     ).toxml()
-    
+
     robot_description = {"robot_description": robot_description_content}
 
     # Node for robot state publisher
     node_robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         output="screen",
-        parameters=[
-            robot_description,
-            {"use_sim_time": True}
-        ],
+        parameters=[robot_description, {"use_sim_time": True}],
     )
 
     # Gazebo Sim (Harmonic) - with environment variables
@@ -118,23 +126,22 @@ def generate_launch_description():
         f'export GZ_SIM_SYSTEM_PLUGIN_PATH="{gz_env["GZ_SIM_SYSTEM_PLUGIN_PATH"]}" && '
         f'export GZ_SIM_RESOURCE_PATH="{gz_env["GZ_SIM_RESOURCE_PATH"]}" && '
         f'export LD_LIBRARY_PATH="{gz_env["LD_LIBRARY_PATH"]}" && '
-        'gz sim -r -v 4 empty.sdf'
-    )
-    
-    gazebo = ExecuteProcess(
-        cmd=['bash', '-c', gz_cmd],
-        output='screen',
-        shell=False
+        "gz sim -r -v 4 empty.sdf"
     )
 
+    gazebo = ExecuteProcess(cmd=["bash", "-c", gz_cmd], output="screen", shell=False)
+
     # Spawn robot
     spawn_entity = Node(
         package="ros_gz_sim",
         executable="create",
         arguments=[
-            "-topic", "robot_description",
-            "-name", "ur5_robotiq",
-            "-allow_renaming", "true",
+            "-topic",
+            "robot_description",
+            "-name",
+            "ur5_robotiq",
+            "-allow_renaming",
+            "true",
         ],
         output="screen",
     )
@@ -152,7 +159,11 @@ def generate_launch_description():
     load_joint_state_broadcaster = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
         output="screen",
         parameters=[{"use_sim_time": True}],
     )
@@ -165,7 +176,7 @@ def generate_launch_description():
         output="screen",
         parameters=[{"use_sim_time": True}],
     )
-    
+
     # Load gripper controller
     load_gripper_controller = Node(
         package="controller_manager",
@@ -179,7 +190,7 @@ def generate_launch_description():
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare("ur5_gripper_description"), "rviz", "view_robot.rviz"]
     )
-    
+
     rviz_node = Node(
         package="rviz2",
         executable="rviz2",
@@ -198,13 +209,15 @@ def generate_launch_description():
         )
     )
 
-    delay_joint_trajectory_controller_after_joint_state_broadcaster = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=load_joint_state_broadcaster,
-            on_exit=[load_joint_trajectory_controller],
+    delay_joint_trajectory_controller_after_joint_state_broadcaster = (
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_broadcaster,
+                on_exit=[load_joint_trajectory_controller],
+            )
         )
     )
-    
+
     delay_gripper_controller_after_joint_trajectory = RegisterEventHandler(
         event_handler=OnProcessExit(
             target_action=load_joint_trajectory_controller,