Use composition for ros_gz and Gazebo in Nav2 launch files (#4)

* Reimplements the turtlebot4 launch file from nav2_bringup in order to
use composition for ros_gz and Gazebo. Hopefully, some of this will be
upstreamed to Nav2.

* This also updates the step size to 0.004 to speed up simulation

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

ionic_demo/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR)
 project(ionic_demo)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
+install(DIRECTORY configs DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME})

ionic_demo/configs/tb4_camera_bridge.yaml

@@ -0,0 +1,12 @@
+# replace clock_bridge
+- topic_name: "/rgbd_camera/image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+  lazy: true
+
+- topic_name: "/rgbd_camera/depth_image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+  lazy: true

ionic_demo/launch/ionic_navigation_demo_launch.py

@@ -1,36 +1,65 @@
-from launch_ros.substitutions import FindPackageShare
+# Copyright (C) 2024 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+)
+from launch.conditions import UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import PathJoinSubstitution, TextSubstitution
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
+    bringup_dir = Path(get_package_share_directory('nav2_bringup'))
+    ionic_demo_dir = Path(get_package_share_directory('ionic_demo'))
+    launch_dir = bringup_dir / 'launch'
+    headless = LaunchConfiguration('headless')
+    declare_headless_cmd = DeclareLaunchArgument(
+        'headless', default_value='True', description='Whether to execute gzclient)'
+    )
 
-    return LaunchDescription([
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([
-                PathJoinSubstitution([
-                    FindPackageShare('nav2_bringup'),
-                    'launch',
-                    'tb4_simulation_launch.py'
-                ])
-            ]),
-            launch_arguments={
-                'map': PathJoinSubstitution([
-                   FindPackageShare('ionic_demo'),
-                      'maps',
-                      'ionic_demo.yaml'
-                ]),
-                'world': PathJoinSubstitution([
-                   FindPackageShare('ionic_demo'),
-                      'worlds',
-                      'ionic.sdf'
-                ]),
-                'x_pose': '0.0',
-                'y_pose': '0.0',
-                'z_pose': '0.0'
-            }.items()
-        )
-    ])
+    return LaunchDescription(
+        [
+            declare_headless_cmd,
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(ionic_demo_dir / 'launch' / 'tb4_spawn_launch.py')
+                ),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(ionic_demo_dir / 'launch' / 'world_launch.py')
+                ),
+            ),
+            ExecuteProcess(
+                cmd=['gz', 'sim', '-g', '-v4'],
+                output='screen',
+                condition=UnlessCondition(headless),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(str(launch_dir / 'rviz_launch.py')),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'rviz_config': str(bringup_dir / 'rviz' / 'nav2_default_view.rviz'),
+                }.items(),
+            ),
+        ]
+    )

ionic_demo/launch/tb4_spawn_launch.py

@@ -0,0 +1,114 @@
+# Copyright (C) 2024 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+from ros_gz_bridge.actions import RosGzBridge
+
+from launch import LaunchDescription
+from launch.actions import (
+    AppendEnvironmentVariable,
+    IncludeLaunchDescription,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    # Get the launch directory
+    ionic_demo_dir = Path(get_package_share_directory('ionic_demo'))
+    bringup_dir = Path(get_package_share_directory('nav2_bringup'))
+    launch_dir = bringup_dir / 'launch'
+    map_yaml_file = str(ionic_demo_dir / 'maps' / 'ionic_demo.yaml')
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(str(launch_dir / 'bringup_launch.py')),
+        launch_arguments={
+            'map': map_yaml_file,
+            'use_sim_time': 'True',
+            'use_composition': 'True',
+        }.items(),
+    )
+    # This checks that tb4 exists needed for the URDF / simulation files.
+    # If not using TB4, its safe to remove.
+    sim_dir = Path(get_package_share_directory('nav2_minimal_tb4_sim'))
+    desc_dir = Path(get_package_share_directory('nav2_minimal_tb4_description'))
+
+    # Launch configuration variables specific to simulation
+    robot_name = 'ionic_tb4'
+    robot_sdf = xacro.process(desc_dir / 'urdf' / 'standard' / 'turtlebot4.urdf.xacro')
+
+    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
+
+    start_robot_state_publisher_cmd = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[
+            {
+                'use_sim_time': True,
+                'robot_description': robot_sdf
+            }
+        ],
+        remappings=remappings,
+    )
+
+    bridge = RosGzBridge(
+        bridge_name='tb4_bridge',
+        config_file=str(sim_dir / 'configs' / 'tb4_bridge.yaml'),
+        use_composition='True',
+    )
+    camera_bridge = RosGzBridge(
+        bridge_name='tb4_camera_bridge',
+        config_file=str(ionic_demo_dir / 'configs' / 'tb4_camera_bridge.yaml'),
+        use_composition='True',
+    )
+
+    spawn_model = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        parameters=[
+            {
+                'entity': robot_name,
+                'string': robot_sdf,
+            }
+        ],
+    )
+
+    set_env_vars_resources = AppendEnvironmentVariable(
+        'GZ_SIM_RESOURCE_PATH', str(desc_dir.parent.resolve())
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    ld.add_action(bridge)
+    ld.add_action(camera_bridge)
+
+    # The meshes used for the turtlebot4 visuals seem to be complex and cause
+    # a drop in RTF on non-GPU machines.
+    # Comment this to let gazebo to fail to find the mesh files
+    ld.add_action(set_env_vars_resources)
+
+    ld.add_action(spawn_model)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(bringup_cmd)
+
+    return ld

ionic_demo/launch/world_launch.py

@@ -0,0 +1,26 @@
+# Copyright (C) 2024 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from ros_gz_sim.actions import GzServer
+
+from launch import LaunchDescription
+
+
+def generate_launch_description():
+    ionic_demo_dir = Path(get_package_share_directory("ionic_demo"))
+    world = str(ionic_demo_dir / "worlds" / "ionic.sdf")
+    return LaunchDescription([GzServer(world_sdf_file=world, use_composition='True')])

ionic_demo/worlds/ionic.sdf

@@ -2,6 +2,10 @@
 
 <sdf version="1.10">
   <world name="default">
+    <physics name="fast" type="ignored">
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>1</real_time_factor>
+    </physics>
     <gui fullscreen="0">
       <plugin filename="MinimalScene" name="3D View">
         <gz-gui>