Add odometry plugin to mobile_robot demos

doc/index.rst

@@ -133,7 +133,7 @@ URDF:
 .. code-block:: xml
 
   <gazebo>
-    <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
       <parameters>$(find gz_ros2_control_demos)/config/cart_controller.yaml</parameters>
     </plugin>
   </gazebo>
@@ -142,7 +142,7 @@ SDF:
 
 .. code-block:: xml
 
-  <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+  <plugin filename="gz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
     <parameters>$(find gz_ros2_control_demos)/config/cart_controller.yaml</parameters>
   </plugin>
 
@@ -172,7 +172,7 @@ URDF:
 .. code-block:: xml
 
   <gazebo>
-    <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
       ...
       <ros>
         <namespace>my_namespace</namespace>
@@ -185,7 +185,7 @@ SDF:
 
 .. code-block:: xml
 
-  <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+  <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
     ...
     <ros>
       <namespace>my_namespace</namespace>
@@ -226,7 +226,7 @@ URDF:
     ...
   <ros2_control>
   <gazebo>
-    <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
+    <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="gz_ros2_control-system">
       ...
     </plugin>
   </gazebo>
@@ -241,7 +241,7 @@ SDF:
     </hardware>
     ...
   <ros2_control>
-  <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
+  <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="gz_ros2_control-system">
     ...
   </plugin>
 
@@ -356,6 +356,10 @@ You can also manually publish on the ``/cmd_vel`` topic to drive the robots:
       y: 0.0
       z: 1.0"
 
+For these demos you can verify that the robot is moving at the desired velocities be echoing the ``/gz/odom`` topic.
+This topic gets the simulation-real odometry of the robot from Gazebo.
+You can also echo the ``<controller_name>/odom`` topic to verify how accurate the odometry calculated by the controller is in comparison to the odometry from ``/gz/odom``.
+
 Gripper
 -----------------------------------------------------------
 

gz_ros2_control_demos/config/diff_drive_controller.yaml

@@ -5,14 +5,13 @@
     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
     wheel_radius: 0.3
 
     wheel_separation_multiplier: 1.0
@@ -29,8 +28,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: "/gz/odom"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS

gz_ros2_control_demos/launch/ackermann_drive_example.launch.py

@@ -12,59 +12,48 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from ament_index_python.packages import get_package_share_directory
 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.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-
 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
 
 
 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(
+        xacro_processed = Command(
             [
                 PathJoinSubstitution([FindExecutable(name='xacro')]),
                 ' ',
                 PathJoinSubstitution([
-                    FindPackageShare('gz_ros2_control_demos'),
-                    performed_description_format,
-                    f'test_ackermann_drive.xacro.{performed_description_format}'
+                    pkg_share,
+                    description_format,
+                    f'test_ackermann_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',
-            'ackermann_drive_controller.yaml',
-        ]
-    )
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'ackermann', '-allow_renaming', 'true'],
-    )
+    robot_controllers = PathJoinSubstitution([
+        pkg_share,
+        'config',
+        'ackermann_drive_controller.yaml'
+    ])
 
     joint_state_broadcaster_spawner = Node(
         package='controller_manager',
@@ -74,45 +63,32 @@ def robot_state_publisher(context):
     ackermann_steering_controller_spawner = Node(
         package='controller_manager',
         executable='spawner',
-        arguments=['ackermann_steering_controller',
-                   '--param-file',
-                   robot_controllers,
-                   '--controller-ros-args',
-                   '-r /ackermann_steering_controller/tf_odometry:=/tf',
-                   '-r /ackermann_steering_controller/reference:=/cmd_vel'
-                   ],
+        arguments=[
+            'ackermann_steering_controller',
+            '--param-file', robot_controllers,
+            '--controller-ros-args',
+            '-r /ackermann_steering_controller/tf_odometry:=/tf',
+            '-r /ackermann_steering_controller/reference:=/cmd_vel'
+        ],
     )
 
-    # Bridge
-    bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-        output='screen'
+    # 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=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']),
+        container_name='ros_gz_container',
+        create_own_container='False',
+        use_composition='True',
     )
 
     ld = LaunchDescription([
-        bridge,
-        # 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=[ackermann_steering_controller_spawner],
-            )
-        ),
+        gz_server,
+        gz_gui,
         gz_spawn_entity,
+        ros_gz_bridge,
+        joint_state_broadcaster_spawner,
+        ackermann_steering_controller_spawner,
         # Launch Arguments
         DeclareLaunchArgument(
             'use_sim_time',

gz_ros2_control_demos/launch/diff_drive_example.launch.py

@@ -12,66 +12,55 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from ament_index_python.packages import get_package_share_directory
 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.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-
 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
 
 
 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(
+        xacro_processed = Command(
             [
                 PathJoinSubstitution([FindExecutable(name='xacro')]),
                 ' ',
                 PathJoinSubstitution([
-                    FindPackageShare('gz_ros2_control_demos'),
-                    performed_description_format,
-                    f'test_diff_drive.xacro.{performed_description_format}'
+                    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 = PathJoinSubstitution([
+        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=[
@@ -83,44 +72,31 @@ def robot_state_publisher(context):
         ],
     )
 
-    # Bridge
-    bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-        output='screen'
+    # 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=PathJoinSubstitution([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))