Use xacro for world file (ros-navigation#2)

Signed-off-by: Addisu Z. Taddese <[email protected]>

Author: azeey

nav2_bringup/launch/tb3_gz_robot_launch.py

@@ -17,10 +17,11 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess
 from launch.actions import AppendEnvironmentVariable
+from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
-from launch.substitutions import Command, LaunchConfiguration
+from launch.substitutions import LaunchConfiguration
+
 from launch_ros.actions import Node
 
 
@@ -59,19 +60,19 @@ def generate_launch_description():
         default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.sdf'),
         description='Full path to robot sdf file to spawn the robot in gazebo')
 
-    pkg_nav2_bringup = get_package_share_directory("nav2_bringup")
+    pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
     bridge = Node(
-        package="ros_gz_bridge",
-        executable="parameter_bridge",
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
         parameters=[
             {
-                "config_file": os.path.join(
-                    pkg_nav2_bringup, "params", "turtlebot3_waffle_bridge.yaml"
+                'config_file': os.path.join(
+                    pkg_nav2_bringup, 'params', 'turtlebot3_waffle_bridge.yaml'
                 ),
-                "qos_overrides./tf_static.publisher.durability": "transient_local",
+                'qos_overrides./tf_static.publisher.durability': 'transient_local',
             }
         ],
-        output="screen",
+        output='screen',
     )
 
     spawn_model = Node(
@@ -87,9 +88,10 @@ def generate_launch_description():
             '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]
     )
 
-    set_env_vars_resources = AppendEnvironmentVariable('GZ_SIM_RESOURCE_PATH', 
-            os.path.join(get_package_share_directory('turtlebot3_gazebo'), 
-                'models'))
+    set_env_vars_resources = AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+            os.path.join(get_package_share_directory('turtlebot3_gazebo'),
+                         'models'))
 
     # Create the launch description and populate
     ld = LaunchDescription()

nav2_bringup/launch/tb3_gz_simulation_launch.py

@@ -15,20 +15,24 @@
 """This is all-in-one launch script intended for use by nav2 developers."""
 
 import os
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch.actions import (
+    AppendEnvironmentVariable,
     DeclareLaunchArgument,
     ExecuteProcess,
     IncludeLaunchDescription,
-    SetEnvironmentVariable,
+    OpaqueFunction,
+    RegisterEventHandler,
 )
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import IfCondition
+from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch.substitutions import Command
+
 from launch_ros.actions import Node
 
 
@@ -159,7 +163,7 @@ def generate_launch_description():
         #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
         # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
         # worlds/turtlebot3_worlds/waffle.model')
-        default_value=os.path.join(bringup_dir, 'worlds', 'gz_world_only.model'),
+        default_value=os.path.join(bringup_dir, 'worlds', 'gz_world_only.sdf.xacro'),
         description='Full path to world model file to load',
     )
 
@@ -215,29 +219,30 @@ def generate_launch_description():
         }.items(),
     )
 
-    gz_sim_server_config = SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', 
-            os.path.join(bringup_dir, 'worlds', 'gz_sim_server_config.xml'), 
-            condition=IfCondition(headless))
-
-    gz_start_scene_broadcaster = SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', 
-            os.path.join(bringup_dir, 'worlds', 'gz_sim_server_scene_broadcaster_config.xml'),
-            condition=UnlessCondition(headless))
-
+    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
+    world_sdf_xacro = ExecuteProcess(
+        cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
     gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(get_package_share_directory('ros_gz_sim'),
-                'launch',
-                'gz_sim.launch.py')
-        ),
-        launch_arguments={'gz_args': ['-r -s ', world]}.items(),
-        condition=IfCondition(use_simulator)
-    )
-
+            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
+                         'gz_sim.launch.py')),
+        launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items(),
+        condition=IfCondition(use_simulator))
+
+    remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
+        on_shutdown=[
+            OpaqueFunction(function=lambda context: os.remove(world_sdf))
+        ]))
+
+    set_env_vars_resources = AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+            os.path.join(get_package_share_directory('turtlebot3_gazebo'),
+                         'models'))
     gazebo_client = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(get_package_share_directory('ros_gz_sim'),
-                'launch',
-                'gz_sim.launch.py')
+                         'launch',
+                         'gz_sim.launch.py')
         ),
         condition=IfCondition(PythonExpression(
             [use_simulator, ' and not ', headless])),
@@ -282,10 +287,10 @@ def generate_launch_description():
     ld.add_action(declare_robot_sdf_cmd)
     ld.add_action(declare_use_respawn_cmd)
 
-
+    ld.add_action(set_env_vars_resources)
+    ld.add_action(world_sdf_xacro)
+    ld.add_action(remove_temp_sdf_file)
     ld.add_action(gz_robot)
-    ld.add_action(gz_sim_server_config)
-    ld.add_action(gz_start_scene_broadcaster)
     ld.add_action(gazebo_server)
     ld.add_action(gazebo_client)
 

nav2_bringup/launch/tb3_simulation_launch.py

@@ -176,13 +176,10 @@ def generate_launch_description():
         condition=IfCondition(use_simulator),
         cmd=[
             'gzserver',
-            '--verbose',
             '-s',
             'libgazebo_ros_init.so',
             '-s',
             'libgazebo_ros_factory.so',
-            '-e',
-            'dart',
             world,
         ],
         cwd=[launch_dir],

nav2_bringup/urdf/gz_turtlebot3_waffle.urdf

@@ -277,7 +277,7 @@
   <gazebo reference="base_scan">
     <sensor name='gpu_lidar' type='gpu_lidar'>"
       <topic>scan</topic>
-      <ignition_frame_id>base_scan</ignition_frame_id>
+      <gz_frame_id>base_scan</gz_frame_id>
       <pose>-0.064 0 0.15 0 0 0</pose>
       <update_rate>5</update_rate>
       <ray>
@@ -314,7 +314,7 @@
       <always_on>true</always_on>
       <update_rate>200</update_rate>
       <topic>imu</topic>
-      <ignition_frame_id>imu_link</ignition_frame_id>
+      <gz_frame_id>imu_link</gz_frame_id>
       <imu>
         <angular_velocity>
           <x>
@@ -362,7 +362,7 @@
 
   <gazebo>
     <plugin
-      filename="ignition-gazebo-diff-drive-system"
+      filename="gz-sim-diff-drive-system"
       name="gz::sim::systems::DiffDrive">
       <left_joint>wheel_left_joint</left_joint>
       <right_joint>wheel_right_joint</right_joint>
@@ -385,8 +385,8 @@
     </plugin>
 
     <plugin
-      filename="ignition-gazebo-pose-publisher-system"
-      name="ignition::gazebo::systems::PosePublisher">
+      filename="gz-sim-pose-publisher-system"
+      name="gz::sim::systems::PosePublisher">
       <publish_link_pose>true</publish_link_pose>
       <publish_sensor_pose>true</publish_sensor_pose>
       <publish_collision_pose>false</publish_collision_pose>

nav2_bringup/worlds/gz_sim_server_config.xml

@@ -37,6 +37,9 @@
               <scale>0.001 0.001 0.001</scale>
             </mesh>
           </geometry>
+          <material>
+            <diffuse>1 1 1</diffuse>
+          </material>
         </visual>
       </link>
 
@@ -45,7 +48,6 @@
           <always_on>true</always_on>
           <update_rate>200</update_rate>
           <topic>imu</topic>
-          <ignition_frame_id>imu_link</ignition_frame_id>
           <gz_frame_id>imu_link</gz_frame_id>
           <imu>
             <angular_velocity>
@@ -124,6 +126,9 @@
               <scale>0.001 0.001 0.001</scale>
             </mesh>
           </geometry>
+          <material>
+            <diffuse>1 1 1</diffuse>
+          </material>
         </visual>
 
         <sensor name="hls_lfcd_lds" type="gpu_lidar">
@@ -132,7 +137,6 @@
           <pose>-0.064 0 0.15 0 0 0</pose>
           <update_rate>5</update_rate>
           <topic>scan</topic>
-          <ignition_frame_id>base_scan</ignition_frame_id>
           <gz_frame_id>base_scan</gz_frame_id>
           <ray>
             <scan>
@@ -211,6 +215,9 @@
               <scale>0.001 0.001 0.001</scale>
             </mesh>
           </geometry>
+          <material>
+            <diffuse>1 1 1</diffuse>
+          </material>
         </visual>
       </link>
 
@@ -268,6 +275,9 @@
               <scale>0.001 0.001 0.001</scale>
             </mesh>
           </geometry>
+          <material>
+            <diffuse>1 1 1</diffuse>
+          </material>
         </visual>
       </link>
 
@@ -367,7 +377,6 @@
           <always_on>1</always_on>
           <update_rate>5</update_rate>
           <pose>0.064 -0.047 0.107 0 0 0</pose>
-          <ignition_frame_id>camera_depth_frame</ignition_frame_id>
           <gz_frame_id>camera_depth_frame</gz_frame_id>
           <camera name="realsense_depth_camera">
             <horizontal_fov>1.047</horizontal_fov>

nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml

@@ -1,6 +1,32 @@
 <?xml version="1.0"?>
-<sdf version="1.6">
+<sdf version="1.6" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:arg name="headless" default="true"/>
+
   <world name="default">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+  <xacro:unless value="$(arg headless)">
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+  </xacro:unless>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin>
+
     <light name='sun' type='directional'>
         <cast_shadows>0</cast_shadows>
         <pose>0 0 10 0 0 0</pose>