Cleanup code and use composable nodes for Gazebo

gz_ros2_control_demos/config/diff_drive_controller.yaml

@@ -5,14 +5,14 @@
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-/**/diff_drive_base_controller:
+/**/diff_drive_controller:
   ros__parameters:
     type: diff_drive_controller/DiffDriveController
     left_wheel_names: ["left_wheel_joint"]
     right_wheel_names: ["right_wheel_joint"]
 
     wheel_separation: 1.25
-    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheels_per_side: 1
     wheel_radius: 0.3
 
     wheel_separation_multiplier: 1.0
@@ -29,8 +29,8 @@
     enable_odom_tf: true
 
     cmd_vel_timeout: 0.5
-    #publish_limited_velocity: true
-    #velocity_rolling_window_size: 10
+    publish_limited_velocity: true
+    velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits
     # Whenever a min_* is unspecified, default to -max_*

gz_ros2_control_demos/config/ros_gz_bridge_config.yaml

@@ -0,0 +1,13 @@
+---
+- ros_topic_name: "/clock"
+  gz_topic_name: "/clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS
+
+# Topic published by OdometryPublisher plugin, only for debugging purposes
+- ros_topic_name: "/gz/odom"
+  gz_topic_name: "/model/diff_drive/odometry"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS

gz_ros2_control_demos/examples/example_diff_drive.cpp

@@ -28,7 +28,7 @@ int main(int argc, char * argv[])
     std::make_shared<rclcpp::Node>("diff_drive_test_node");
 
   auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
-    "/diff_drive_base_controller/cmd_vel", 10);
+    "/diff_drive_controller/cmd_vel", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 

gz_ros2_control_demos/launch/diff_drive_example.launch.py

@@ -12,113 +12,102 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-from launch.actions import RegisterEventHandler
-from launch.event_handlers import OnProcessExit
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+import os
 
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
+from ros_gz_bridge.actions import RosGzBridge
+from ros_gz_sim.actions import GzServer
+import xacro
 
 
 def generate_launch_description():
+    pkg_share = get_package_share_directory('gz_ros2_control_demos')
+
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
     def robot_state_publisher(context):
-        performed_description_format = LaunchConfiguration('description_format').perform(context)
+        description_format = LaunchConfiguration('description_format').perform(context)
         # Get URDF or SDF via xacro
-        robot_description_content = Command(
-            [
-                PathJoinSubstitution([FindExecutable(name='xacro')]),
-                ' ',
-                PathJoinSubstitution([
-                    FindPackageShare('gz_ros2_control_demos'),
-                    performed_description_format,
-                    f'test_diff_drive.xacro.{performed_description_format}'
-                ]),
-            ]
+        xacro_processed = xacro.process(
+            os.path.join(
+                pkg_share,
+                description_format,
+                f'test_diff_drive.xacro.{description_format}'
+            )
         )
-        robot_description = {'robot_description': robot_description_content}
         node_robot_state_publisher = Node(
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            parameters=[robot_description]
+            parameters=[{'robot_description': xacro_processed}]
         )
         return [node_robot_state_publisher]
 
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare('gz_ros2_control_demos'),
-            'config',
-            'diff_drive_controller.yaml',
-        ]
-    )
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'diff_drive', '-allow_renaming', 'true'],
+    robot_controllers = os.path.join(
+        pkg_share,
+        'config',
+        'diff_drive_controller.yaml'
     )
 
     joint_state_broadcaster_spawner = Node(
         package='controller_manager',
         executable='spawner',
         arguments=['joint_state_broadcaster'],
     )
-    diff_drive_base_controller_spawner = Node(
+    diff_drive_controller_spawner = Node(
         package='controller_manager',
         executable='spawner',
-        arguments=[
-            'diff_drive_base_controller',
-            '--param-file',
-            robot_controllers,
-            ],
+        arguments=['diff_drive_controller', '--param-file', robot_controllers],
     )
 
-    # Bridge
-    bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-        output='screen'
+    # Launch just the Gazebo server as a composable node.
+    gz_server = GzServer(
+        world_sdf_file='empty.sdf',
+        container_name='ros_gz_container',
+        create_own_container='True',
+        use_composition='True',
+    )
+
+    gz_gui = ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen')
+
+    gz_spawn_entity = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        arguments=['-topic', 'robot_description', '-name',
+                   'diff_drive', '-allow_renaming', 'true'],
+    )
+
+    # Setup ros_gz_bridge to bridge topics between ROS and Gazebo.
+    # It is launched as a composable node in the container created by the Gazebo server.
+    ros_gz_bridge = RosGzBridge(
+        bridge_name='ros_gz_bridge',
+        config_file=os.path.join(pkg_share, 'config', 'ros_gz_bridge_config.yaml'),
+        container_name='ros_gz_container',
+        create_own_container='False',
+        use_composition='True',
     )
 
     ld = LaunchDescription([
-        # Launch gazebo environment
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]),
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=gz_spawn_entity,
-                on_exit=[joint_state_broadcaster_spawner],
-            )
-        ),
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=joint_state_broadcaster_spawner,
-                on_exit=[diff_drive_base_controller_spawner],
-            )
-        ),
-        bridge,
+        gz_server,
+        gz_gui,
         gz_spawn_entity,
+        ros_gz_bridge,
+        joint_state_broadcaster_spawner,
+        diff_drive_controller_spawner,
         # Launch Arguments
         DeclareLaunchArgument(
             'use_sim_time',
             default_value=use_sim_time,
             description='If true, use simulated clock'),
         DeclareLaunchArgument(
             'description_format',
-            default_value='urdf',
+            default_value='sdf',
             description='Robot description format to use, urdf or sdf'),
     ])
     ld.add_action(OpaqueFunction(function=robot_state_publisher))