Upgrading simple commander to use new gazebo simulation environment, gazebo harmonic, and new relavent poses

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v24\
+              - "<< parameters.key >>-v25\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v24\
+              - "<< parameters.key >>-v25\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v24\
+            key: "<< parameters.key >>-v25\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_bringup/launch/tb4_simulation_launch.py

@@ -207,6 +207,7 @@ def generate_launch_description():
             'use_respawn': use_respawn,
         }.items(),
     )
+
     # The SDF file for the world is a xacro file because we wanted to
     # conditionally load the SceneBroadcaster plugin based on wheter we're
     # running in headless mode. But currently, the Gazebo command line doesn't

nav2_simple_commander/launch/assisted_teleop_example_launch.py

@@ -14,28 +14,35 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch.actions import (
+    AppendEnvironmentVariable,
     DeclareLaunchArgument,
     ExecuteProcess,
     IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
 )
 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, LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
-    python_commander_dir = get_package_share_directory('nav2_simple_commander')
+    sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
+    desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
 
-    map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
-    world = os.path.join(python_commander_dir, 'warehouse.world')
+    robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro')
+    world = os.path.join(sim_dir, 'worlds', 'depot.sdf')
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml')
 
     # Launch configuration variables
     use_rviz = LaunchConfiguration('use_rviz')
@@ -51,26 +58,54 @@ def generate_launch_description():
     )
 
     # start the simulation
-    start_gazebo_server_cmd = ExecuteProcess(
-        cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
-        cwd=[warehouse_dir],
-        output='screen',
+    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
+    world_sdf_xacro = ExecuteProcess(
+        cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
+    start_gazebo_server_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
+                         'gz_sim.launch.py')),
+        launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
+
+    remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
+        on_shutdown=[
+            OpaqueFunction(function=lambda _: os.remove(world_sdf))
+        ]))
+
+    set_env_vars_resources = AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+            os.path.join(sim_dir, 'worlds'))
+    start_gazebo_client_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'),
+                         'launch',
+                         'gz_sim.launch.py')
+        ),
+        condition=IfCondition(PythonExpression(
+            ['not ', headless])),
+        launch_arguments={'gz_args': ['-v4 -g ']}.items(),
     )
 
-    start_gazebo_client_cmd = ExecuteProcess(
-        condition=IfCondition(PythonExpression(['not ', headless])),
-        cmd=['gzclient'],
-        cwd=[warehouse_dir],
-        output='screen',
-    )
+    spawn_robot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')),
+        launch_arguments={'use_sim_time': 'True',
+                          'robot_sdf': robot_sdf,
+                          'x_pose': '-8.0',
+                          'y_pose': '0.0',
+                          'z_pose': '0.0',
+                          'roll': '0.0',
+                          'pitch': '0.0',
+                          'yaw': '0.0'}.items())
 
-    urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
     start_robot_state_publisher_cmd = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
-        arguments=[urdf],
+        parameters=[
+            {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])}
+        ]
     )
 
     # start the visualization
@@ -87,7 +122,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
         ),
-        launch_arguments={'map': map_yaml_file}.items(),
+        launch_arguments={'map': map_yaml_file, 'use_sim_time': 'True'}.items(),
     )
 
     # start the demo autonomy task
@@ -98,11 +133,20 @@ def generate_launch_description():
         output='screen',
     )
 
+    set_env_vars_resources2 = AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+            str(Path(os.path.join(desc_dir)).parent.resolve()))
+
     ld = LaunchDescription()
     ld.add_action(declare_use_rviz_cmd)
     ld.add_action(declare_simulator_cmd)
+    ld.add_action(world_sdf_xacro)
+    ld.add_action(remove_temp_sdf_file)
+    ld.add_action(set_env_vars_resources)
+    ld.add_action(set_env_vars_resources2)
     ld.add_action(start_gazebo_server_cmd)
     ld.add_action(start_gazebo_client_cmd)
+    ld.add_action(spawn_robot_cmd)
     ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(rviz_cmd)
     ld.add_action(bringup_cmd)

nav2_simple_commander/launch/follow_path_example_launch.py

@@ -13,28 +13,35 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch.actions import (
+    AppendEnvironmentVariable,
     DeclareLaunchArgument,
     ExecuteProcess,
     IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
 )
 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, LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
-    python_commander_dir = get_package_share_directory('nav2_simple_commander')
+    sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
+    desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
 
-    map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
-    world = os.path.join(python_commander_dir, 'warehouse.world')
+    robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro')
+    world = os.path.join(sim_dir, 'worlds', 'depot.sdf')
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml')
 
     # Launch configuration variables
     use_rviz = LaunchConfiguration('use_rviz')
@@ -50,26 +57,54 @@ def generate_launch_description():
     )
 
     # start the simulation
-    start_gazebo_server_cmd = ExecuteProcess(
-        cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
-        cwd=[warehouse_dir],
-        output='screen',
+    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
+    world_sdf_xacro = ExecuteProcess(
+        cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world])
+    start_gazebo_server_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
+                         'gz_sim.launch.py')),
+        launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
+
+    remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
+        on_shutdown=[
+            OpaqueFunction(function=lambda _: os.remove(world_sdf))
+        ]))
+
+    set_env_vars_resources = AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+            os.path.join(sim_dir, 'worlds'))
+    start_gazebo_client_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'),
+                         'launch',
+                         'gz_sim.launch.py')
+        ),
+        condition=IfCondition(PythonExpression(
+            ['not ', headless])),
+        launch_arguments={'gz_args': ['-v4 -g ']}.items(),
     )
 
-    start_gazebo_client_cmd = ExecuteProcess(
-        condition=IfCondition(PythonExpression(['not ', headless])),
-        cmd=['gzclient'],
-        cwd=[warehouse_dir],
-        output='screen',
-    )
+    spawn_robot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')),
+        launch_arguments={'use_sim_time': 'True',
+                          'robot_sdf': robot_sdf,
+                          'x_pose': '-8.0',
+                          'y_pose': '0.0',
+                          'z_pose': '0.0',
+                          'roll': '0.0',
+                          'pitch': '0.0',
+                          'yaw': '0.0'}.items())
 
-    urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
     start_robot_state_publisher_cmd = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
-        arguments=[urdf],
+        parameters=[
+            {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])}
+        ]
     )
 
     # start the visualization
@@ -97,11 +132,20 @@ def generate_launch_description():
         output='screen',
     )
 
+    set_env_vars_resources2 = AppendEnvironmentVariable(
+            'GZ_SIM_RESOURCE_PATH',
+            str(Path(os.path.join(desc_dir)).parent.resolve()))
+
     ld = LaunchDescription()
     ld.add_action(declare_use_rviz_cmd)
     ld.add_action(declare_simulator_cmd)
+    ld.add_action(world_sdf_xacro)
+    ld.add_action(remove_temp_sdf_file)
+    ld.add_action(set_env_vars_resources)
+    ld.add_action(set_env_vars_resources2)
     ld.add_action(start_gazebo_server_cmd)
     ld.add_action(start_gazebo_client_cmd)
+    ld.add_action(spawn_robot_cmd)
     ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(rviz_cmd)
     ld.add_action(bringup_cmd)