From c05345b31c73c91fe67b0938dba8d837e8ce707e Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Mon, 10 Mar 2025 08:49:16 +0530 Subject: [PATCH 01/13] Cleanup code and use composable nodes for Gazebo - diff_drive_controller --- .../config/diff_drive_controller.yaml | 8 +- .../config/ros_gz_bridge_config.yaml | 13 ++ .../examples/example_diff_drive.cpp | 2 +- .../launch/diff_drive_example.launch.py | 122 +++++++-------- .../diff_drive_example_namespaced.launch.py | 148 +++++++++--------- gz_ros2_control_demos/package.xml | 2 + .../sdf/test_diff_drive.xacro.sdf | 13 +- 7 files changed, 161 insertions(+), 147 deletions(-) create mode 100644 gz_ros2_control_demos/config/ros_gz_bridge_config.yaml diff --git a/gz_ros2_control_demos/config/diff_drive_controller.yaml b/gz_ros2_control_demos/config/diff_drive_controller.yaml index 02446dff..ccb359d9 100644 --- a/gz_ros2_control_demos/config/diff_drive_controller.yaml +++ b/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_* diff --git a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml new file mode 100644 index 00000000..7114a70a --- /dev/null +++ b/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 diff --git a/gz_ros2_control_demos/examples/example_diff_drive.cpp b/gz_ros2_control_demos/examples/example_diff_drive.cpp index 6858564c..d7d1e14e 100644 --- a/gz_ros2_control_demos/examples/example_diff_drive.cpp +++ b/gz_ros2_control_demos/examples/example_diff_drive.cpp @@ -28,7 +28,7 @@ int main(int argc, char * argv[]) std::make_shared("diff_drive_test_node"); auto publisher = node->create_publisher( - "/diff_drive_base_controller/cmd_vel", 10); + "/diff_drive_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index 9c79d9e0..493b3ccc 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -12,58 +12,45 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os +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.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - +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( @@ -71,46 +58,47 @@ def robot_state_publisher(context): 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], + ) + + # 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'], ) - # 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=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', @@ -118,7 +106,7 @@ def robot_state_publisher(context): 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)) diff --git a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py index 3e738b80..4b506e09 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py @@ -12,58 +12,47 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -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 - +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) - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - ' ', - PathJoinSubstitution( - [FindPackageShare('gz_ros2_control_demos'), - 'urdf', 'test_diff_drive.xacro.urdf'] + def robot_state_publisher(context): + description_format = LaunchConfiguration('description_format').perform(context) + # Get URDF or SDF via xacro + xacro_processed = xacro.process( + os.path.join( + pkg_share, + description_format, + f'test_diff_drive.xacro.{description_format}' ), - ' ', - 'namespace:=r1', - ] - ) - robot_description = {'robot_description': robot_description_content} - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare('gz_ros2_control_demos'), - 'config', - 'diff_drive_controller.yaml', - ] - ) - - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - namespace='r1', - output='screen', - parameters=[robot_description] - ) + mappings={"namespace": "r1"} + ) + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace='r1', + output='screen', + parameters=[{'robot_description': xacro_processed}] + ) + return [node_robot_state_publisher] - gz_spawn_entity = Node( - package='ros_gz_sim', - executable='create', - namespace='r1', - 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( @@ -72,53 +61,64 @@ def generate_launch_description(): arguments=[ 'joint_state_broadcaster', '-c', '/r1/controller_manager' - ], + ], ) - diff_drive_base_controller_spawner = Node( + diff_drive_controller_spawner = Node( package='controller_manager', executable='spawner', arguments=[ - 'diff_drive_base_controller', + 'diff_drive_controller', '--param-file', robot_controllers, '-c', '/r1/controller_manager' - ], + ], + ) + + # 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', + namespace='r1', + output='screen', + arguments=['-topic', 'robot_description', '-name', + 'diff_drive', '-allow_renaming', 'true'], ) - # 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=os.path.join(pkg_share, 'config', 'ros_gz_bridge_config.yaml'), + container_name='ros_gz_container', + create_own_container='False', + use_composition='True', ) - return 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, - node_robot_state_publisher, + ld = LaunchDescription([ + 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='sdf', + description='Robot description format to use, urdf or sdf'), ]) + ld.add_action(OpaqueFunction(function=robot_state_publisher)) + return ld diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index 80be55d4..6f54712a 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -20,6 +20,8 @@ geometry_msgs rclcpp rclcpp_action + ros_gz_bridge + ros_gz_sim std_msgs ament_index_python diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf index 0e7f2bad..83822ff7 100644 --- a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -1,6 +1,9 @@ + + + true @@ -212,7 +215,15 @@ - $(find gz_ros2_control_demos)/config/diff_drive_controller_velocity.yaml + + $(arg namespace) + + $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml + + + + diff_drive/odom + diff_drive From 82cfe3d674423a0e735f117b6fae51d1a1a10d9a Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Mon, 10 Mar 2025 09:01:52 +0530 Subject: [PATCH 02/13] Fix whitespace --- gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf index 83822ff7..04141e16 100644 --- a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -1,7 +1,7 @@ - + From e9f25585d184593dfc16b3d626330a874543a55e Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Mon, 10 Mar 2025 09:06:41 +0530 Subject: [PATCH 03/13] Fix code formatting --- gz_ros2_control_demos/launch/diff_drive_example.launch.py | 1 + .../launch/diff_drive_example_namespaced.launch.py | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index 493b3ccc..ee1ab632 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction diff --git a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py index 4b506e09..cbef93e8 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction @@ -38,7 +39,7 @@ def robot_state_publisher(context): description_format, f'test_diff_drive.xacro.{description_format}' ), - mappings={"namespace": "r1"} + mappings={'namespace': 'r1'} ) node_robot_state_publisher = Node( package='robot_state_publisher', From cc04e701fd8df86d9d88c844d4278b1c07873a42 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 15 Mar 2025 11:26:00 +0530 Subject: [PATCH 04/13] Remove wheels_per_side parameter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- gz_ros2_control_demos/config/diff_drive_controller.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/gz_ros2_control_demos/config/diff_drive_controller.yaml b/gz_ros2_control_demos/config/diff_drive_controller.yaml index ccb359d9..ebb160bb 100644 --- a/gz_ros2_control_demos/config/diff_drive_controller.yaml +++ b/gz_ros2_control_demos/config/diff_drive_controller.yaml @@ -12,7 +12,6 @@ right_wheel_names: ["right_wheel_joint"] wheel_separation: 1.25 - wheels_per_side: 1 wheel_radius: 0.3 wheel_separation_multiplier: 1.0 From 084b5d435479cfd2ff815677edf6a73ce02fbc44 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 26 Mar 2025 13:43:10 +0530 Subject: [PATCH 05/13] Add documentation on verifying odometry from ``/gz/odom``. --- doc/index.rst | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 88e4a5ab..9911d530 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -327,6 +327,7 @@ You can run some of the mobile robots running the following commands: ros2 launch gz_ros2_control_demos diff_drive_example.launch.py ros2 launch gz_ros2_control_demos tricycle_drive_example.launch.py ros2 launch gz_ros2_control_demos ackermann_drive_example.launch.py + ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py When the Gazebo world is launched you can run some of the following commands to move the robots. @@ -336,6 +337,16 @@ When the Gazebo world is launched you can run some of the following commands to ros2 run gz_ros2_control_demos example_tricycle_drive ros2 run gz_ros2_control_demos example_ackermann_drive +You can drive the Mecanum mobile robot from the keyboard using the following command: + +.. code-block:: shell + + ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true + +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 ``/odom`` topic to verify how accurate the odometry calculated by the controller is in comparison to the odometry from ``/gz/odom``. + To demonstrate the setup of a namespaced robot, run .. code-block:: shell @@ -348,13 +359,6 @@ which will launch a diff drive robot within the namespace ``r1``. The ros2_control settings for the controller_manager and the controller defined in ``diff_drive_controller.yaml`` use wildcards to match all namespaces. -To run the Mecanum mobile robot run the following commands to drive it from the keyboard: - -.. code-block:: shell - - ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py - ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true - Gripper ----------------------------------------------------------- From bc65a23046b7b014a9c49e900d263fff17831d4f Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 26 Mar 2025 14:33:07 +0530 Subject: [PATCH 06/13] Update ackermann_drive demo and unify cmd_vel topic for mobile robot demos --- doc/index.rst | 35 +++-- .../config/ros_gz_bridge_config.yaml | 2 +- .../examples/example_ackermann_drive.cpp | 62 --------- ...ff_drive.cpp => example_mobile_robots.cpp} | 4 +- .../examples/example_tricycle_drive.cpp | 54 -------- .../launch/ackermann_drive_example.launch.py | 126 +++++++++--------- .../launch/diff_drive_example.launch.py | 7 +- .../sdf/test_ackermann_drive.xacro.sdf | 6 + .../sdf/test_diff_drive.xacro.sdf | 1 + .../urdf/test_ackermann_drive.xacro.urdf | 6 + .../urdf/test_diff_drive.xacro.urdf | 6 + 11 files changed, 112 insertions(+), 197 deletions(-) delete mode 100644 gz_ros2_control_demos/examples/example_ackermann_drive.cpp rename gz_ros2_control_demos/examples/{example_diff_drive.cpp => example_mobile_robots.cpp} (93%) delete mode 100644 gz_ros2_control_demos/examples/example_tricycle_drive.cpp diff --git a/doc/index.rst b/doc/index.rst index 9911d530..cb06e455 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -133,7 +133,7 @@ URDF: .. code-block:: xml - + $(find gz_ros2_control_demos)/config/cart_controller.yaml @@ -142,7 +142,7 @@ SDF: .. code-block:: xml - + $(find gz_ros2_control_demos)/config/cart_controller.yaml @@ -172,7 +172,7 @@ URDF: .. code-block:: xml - + ... my_namespace @@ -185,7 +185,7 @@ SDF: .. code-block:: xml - + ... my_namespace @@ -226,7 +226,7 @@ URDF: ... - + ... @@ -241,7 +241,7 @@ SDF: ... - + ... @@ -329,20 +329,33 @@ You can run some of the mobile robots running the following commands: ros2 launch gz_ros2_control_demos ackermann_drive_example.launch.py ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py -When the Gazebo world is launched you can run some of the following commands to move the robots. +When the Gazebo world is launched you can run the following command to move the robots. .. code-block:: shell - ros2 run gz_ros2_control_demos example_diff_drive - ros2 run gz_ros2_control_demos example_tricycle_drive - ros2 run gz_ros2_control_demos example_ackermann_drive + ros2 run gz_ros2_control_demos example_mobile_robots -You can drive the Mecanum mobile robot from the keyboard using the following command: +You can also drive the robots from the keyboard using the following command: .. code-block:: shell ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true +You can also manually publish on the ``/cmd_vel`` topic to drive the robots: + +.. code-block:: shell + + ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " + twist: + linear: + x: 0.7 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + 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 ``/odom`` topic to verify how accurate the odometry calculated by the controller is in comparison to the odometry from ``/gz/odom``. diff --git a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml index 7114a70a..30766d5f 100644 --- a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml +++ b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml @@ -7,7 +7,7 @@ # Topic published by OdometryPublisher plugin, only for debugging purposes - ros_topic_name: "/gz/odom" - gz_topic_name: "/model/diff_drive/odometry" + gz_topic_name: "/gz/odom" ros_type_name: "nav_msgs/msg/Odometry" gz_type_name: "gz.msgs.Odometry" direction: GZ_TO_ROS diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp deleted file mode 100644 index 7be1633e..00000000 --- a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2024 Open Source Robotics Foundation, Inc. -// -// 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. - -#include -#include - -#include - -#include - -using namespace std::chrono_literals; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - std::shared_ptr node = - std::make_shared("ackermann_drive_test_node"); - - node->set_parameter(rclcpp::Parameter("use_sim_time", true)); - - auto publisher = node->create_publisher( - "/ackermann_steering_controller/reference", 10); - - RCLCPP_INFO(node->get_logger(), "node created"); - - geometry_msgs::msg::Twist tw; - - tw.linear.x = 0.5; - tw.linear.y = 0.0; - tw.linear.z = 0.0; - - tw.angular.x = 0.0; - tw.angular.y = 0.0; - tw.angular.z = 0.3; - - geometry_msgs::msg::TwistStamped command; - command.twist = tw; - - while (1) { - rclcpp::spin_some(node); - if (node->get_clock()->started()) { - command.header.stamp = node->now(); - publisher->publish(command); - } - std::this_thread::sleep_for(50ms); - } - rclcpp::shutdown(); - - return 0; -} diff --git a/gz_ros2_control_demos/examples/example_diff_drive.cpp b/gz_ros2_control_demos/examples/example_mobile_robots.cpp similarity index 93% rename from gz_ros2_control_demos/examples/example_diff_drive.cpp rename to gz_ros2_control_demos/examples/example_mobile_robots.cpp index d7d1e14e..8c2242c2 100644 --- a/gz_ros2_control_demos/examples/example_diff_drive.cpp +++ b/gz_ros2_control_demos/examples/example_mobile_robots.cpp @@ -25,10 +25,10 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); std::shared_ptr node = - std::make_shared("diff_drive_test_node"); + std::make_shared("example_mobile_robots_node"); auto publisher = node->create_publisher( - "/diff_drive_controller/cmd_vel", 10); + "/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); diff --git a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp deleted file mode 100644 index ff49808b..00000000 --- a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2022 Open Source Robotics Foundation, Inc. -// -// 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. - -#include - -#include - -#include - -using namespace std::chrono_literals; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - std::shared_ptr node = - std::make_shared("tricycle_drive_test_node"); - - auto publisher = node->create_publisher( - "/tricycle_controller/cmd_vel", 10); - - RCLCPP_INFO(node->get_logger(), "node created"); - - geometry_msgs::msg::TwistStamped command; - - command.twist.linear.x = 0.2; - command.twist.linear.y = 0.0; - command.twist.linear.z = 0.0; - - command.twist.angular.x = 0.0; - command.twist.angular.y = 0.0; - command.twist.angular.z = 0.1; - - while (1) { - command.header.stamp = node->now(); - publisher->publish(command); - std::this_thread::sleep_for(50ms); - rclcpp::spin_some(node); - } - rclcpp::shutdown(); - - return 0; -} diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py index 34b9c1f5..a49aca0a 100644 --- a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py @@ -12,58 +12,46 @@ # 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_ackermann_drive.xacro.{performed_description_format}' - ]), - ] + xacro_processed = xacro.process( + os.path.join( + 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 = os.path.join( + pkg_share, + 'config', + 'ackermann_drive_controller.yaml' ) joint_state_broadcaster_spawner = Node( @@ -74,44 +62,50 @@ 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', - ], + 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' + # 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', + 'ackermann_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([ - 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', diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index ee1ab632..c5b2a632 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -62,7 +62,12 @@ def robot_state_publisher(context): diff_drive_controller_spawner = Node( package='controller_manager', executable='spawner', - arguments=['diff_drive_controller', '--param-file', robot_controllers], + arguments=[ + 'diff_drive_controller', + '--param-file', robot_controllers, + '--controller-ros-args', + '-r /diff_drive_controller/cmd_vel:=/cmd_vel' + ], ) # Launch just the Gazebo server as a composable node. diff --git a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf index bca986df..4351ee15 100644 --- a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf @@ -360,5 +360,11 @@ $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml + + ackermann_drive/odom + ackermann_drive + /gz/odom + + diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf index 04141e16..fc3f9b64 100644 --- a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -224,6 +224,7 @@ diff_drive/odom diff_drive + /gz/odom diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf index bcdf8205..26f53293 100644 --- a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf @@ -216,6 +216,12 @@ $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml + + + ackermann_drive/odom + ackermann_drive + /gz/odom + diff --git a/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index 08956f33..98c5f75e 100644 --- a/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -176,6 +176,12 @@ $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml + + + diff_drive/odom + diff_drive + /gz/odom + From ec50591665178b04ab6bb713d7a78f9dd3ef7faa Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 13:15:52 +0530 Subject: [PATCH 07/13] Revert "unify cmd_vel topic for mobile robot demos" --- .../examples/example_ackermann_drive.cpp | 62 +++++++++++++++++++ ...bile_robots.cpp => example_diff_drive.cpp} | 4 +- .../examples/example_tricycle_drive.cpp | 54 ++++++++++++++++ .../launch/ackermann_drive_example.launch.py | 1 - .../launch/diff_drive_example.launch.py | 4 +- 5 files changed, 119 insertions(+), 6 deletions(-) create mode 100644 gz_ros2_control_demos/examples/example_ackermann_drive.cpp rename gz_ros2_control_demos/examples/{example_mobile_robots.cpp => example_diff_drive.cpp} (93%) create mode 100644 gz_ros2_control_demos/examples/example_tricycle_drive.cpp diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp new file mode 100644 index 00000000..7be1633e --- /dev/null +++ b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp @@ -0,0 +1,62 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// 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. + +#include +#include + +#include + +#include + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = + std::make_shared("ackermann_drive_test_node"); + + node->set_parameter(rclcpp::Parameter("use_sim_time", true)); + + auto publisher = node->create_publisher( + "/ackermann_steering_controller/reference", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + geometry_msgs::msg::Twist tw; + + tw.linear.x = 0.5; + tw.linear.y = 0.0; + tw.linear.z = 0.0; + + tw.angular.x = 0.0; + tw.angular.y = 0.0; + tw.angular.z = 0.3; + + geometry_msgs::msg::TwistStamped command; + command.twist = tw; + + while (1) { + rclcpp::spin_some(node); + if (node->get_clock()->started()) { + command.header.stamp = node->now(); + publisher->publish(command); + } + std::this_thread::sleep_for(50ms); + } + rclcpp::shutdown(); + + return 0; +} diff --git a/gz_ros2_control_demos/examples/example_mobile_robots.cpp b/gz_ros2_control_demos/examples/example_diff_drive.cpp similarity index 93% rename from gz_ros2_control_demos/examples/example_mobile_robots.cpp rename to gz_ros2_control_demos/examples/example_diff_drive.cpp index 8c2242c2..d7d1e14e 100644 --- a/gz_ros2_control_demos/examples/example_mobile_robots.cpp +++ b/gz_ros2_control_demos/examples/example_diff_drive.cpp @@ -25,10 +25,10 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); std::shared_ptr node = - std::make_shared("example_mobile_robots_node"); + std::make_shared("diff_drive_test_node"); auto publisher = node->create_publisher( - "/cmd_vel", 10); + "/diff_drive_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); diff --git a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp new file mode 100644 index 00000000..ff49808b --- /dev/null +++ b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp @@ -0,0 +1,54 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// 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. + +#include + +#include + +#include + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = + std::make_shared("tricycle_drive_test_node"); + + auto publisher = node->create_publisher( + "/tricycle_controller/cmd_vel", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + geometry_msgs::msg::TwistStamped command; + + command.twist.linear.x = 0.2; + command.twist.linear.y = 0.0; + command.twist.linear.z = 0.0; + + command.twist.angular.x = 0.0; + command.twist.angular.y = 0.0; + command.twist.angular.z = 0.1; + + while (1) { + command.header.stamp = node->now(); + publisher->publish(command); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node); + } + rclcpp::shutdown(); + + return 0; +} diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py index a49aca0a..1b025574 100644 --- a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py @@ -67,7 +67,6 @@ def robot_state_publisher(context): '--param-file', robot_controllers, '--controller-ros-args', '-r /ackermann_steering_controller/tf_odometry:=/tf', - '-r /ackermann_steering_controller/reference:=/cmd_vel' ], ) diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index c5b2a632..49e1cf84 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -64,9 +64,7 @@ def robot_state_publisher(context): executable='spawner', arguments=[ 'diff_drive_controller', - '--param-file', robot_controllers, - '--controller-ros-args', - '-r /diff_drive_controller/cmd_vel:=/cmd_vel' + '--param-file', robot_controllers ], ) From 68ec05bb39e01c0af3ea0190cbc7cb679e6306fe Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 13:45:09 +0530 Subject: [PATCH 08/13] Revert doc updates --- doc/index.rst | 27 ++++++--------------------- 1 file changed, 6 insertions(+), 21 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index cb06e455..4bd76f0a 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -327,35 +327,20 @@ You can run some of the mobile robots running the following commands: ros2 launch gz_ros2_control_demos diff_drive_example.launch.py ros2 launch gz_ros2_control_demos tricycle_drive_example.launch.py ros2 launch gz_ros2_control_demos ackermann_drive_example.launch.py - ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py -When the Gazebo world is launched you can run the following command to move the robots. +When the Gazebo world is launched you can run some of the following commands to move the robots. .. code-block:: shell + ros2 run gz_ros2_control_demos example_diff_drive + ros2 run gz_ros2_control_demos example_tricycle_drive + ros2 run gz_ros2_control_demos example_ackermann_drive - ros2 run gz_ros2_control_demos example_mobile_robots - -You can also drive the robots from the keyboard using the following command: +To run the Mecanum mobile robot run the following commands to drive it from the keyboard: .. code-block:: shell - + ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true -You can also manually publish on the ``/cmd_vel`` topic to drive the robots: - -.. code-block:: shell - - ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " - twist: - linear: - x: 0.7 - y: 0.0 - z: 0.0 - angular: - x: 0.0 - 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 ``/odom`` topic to verify how accurate the odometry calculated by the controller is in comparison to the odometry from ``/gz/odom``. From ad88a8ead970de902473017b81f44148763e9a2c Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 13:58:01 +0530 Subject: [PATCH 09/13] Pass tests --- doc/index.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/index.rst b/doc/index.rst index 4bd76f0a..60f4c023 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -331,6 +331,7 @@ You can run some of the mobile robots running the following commands: When the Gazebo world is launched you can run some of the following commands to move the robots. .. code-block:: shell + ros2 run gz_ros2_control_demos example_diff_drive ros2 run gz_ros2_control_demos example_tricycle_drive ros2 run gz_ros2_control_demos example_ackermann_drive @@ -338,6 +339,7 @@ When the Gazebo world is launched you can run some of the following commands to To run the Mecanum mobile robot run the following commands to drive it from the keyboard: .. code-block:: shell + ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true From 0310e8162e8a3f403319f2e673297b97c2a22df2 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 14:05:45 +0530 Subject: [PATCH 10/13] Revert adding odometry plugin --- doc/index.rst | 4 ---- gz_ros2_control_demos/config/ros_gz_bridge_config.yaml | 7 ------- gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf | 6 ------ gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf | 6 ------ gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf | 6 ------ gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf | 6 ------ 6 files changed, 35 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 60f4c023..e7ae981f 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -343,10 +343,6 @@ To run the Mecanum mobile robot run the following commands to drive it from the ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true -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 ``/odom`` topic to verify how accurate the odometry calculated by the controller is in comparison to the odometry from ``/gz/odom``. - To demonstrate the setup of a namespaced robot, run .. code-block:: shell diff --git a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml index 30766d5f..e48c37fb 100644 --- a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml +++ b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml @@ -4,10 +4,3 @@ 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 diff --git a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf index 4351ee15..bca986df 100644 --- a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf @@ -360,11 +360,5 @@ $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml - - ackermann_drive/odom - ackermann_drive - /gz/odom - - diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf index fc3f9b64..f836b88a 100644 --- a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -221,11 +221,5 @@ $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml - - diff_drive/odom - diff_drive - /gz/odom - - diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf index 26f53293..bcdf8205 100644 --- a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf @@ -216,12 +216,6 @@ $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml - - - ackermann_drive/odom - ackermann_drive - /gz/odom - diff --git a/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index 98c5f75e..08956f33 100644 --- a/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -176,12 +176,6 @@ $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml - - - diff_drive/odom - diff_drive - /gz/odom - From 1a4ac214ffbca790ca92a35b2e6cb1227f9db40e Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 14:40:18 +0530 Subject: [PATCH 11/13] Apply review comments --- .../launch/ackermann_drive_example.launch.py | 27 +++++++++-------- .../launch/diff_drive_example.launch.py | 27 +++++++++-------- .../diff_drive_example_namespaced.launch.py | 30 ++++++++++--------- 3 files changed, 44 insertions(+), 40 deletions(-) diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py index 1b025574..62a326a6 100644 --- a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py @@ -12,16 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -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.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from ros_gz_bridge.actions import RosGzBridge from ros_gz_sim.actions import GzServer -import xacro def generate_launch_description(): @@ -33,12 +30,16 @@ def generate_launch_description(): def robot_state_publisher(context): description_format = LaunchConfiguration('description_format').perform(context) # Get URDF or SDF via xacro - xacro_processed = xacro.process( - os.path.join( - pkg_share, - description_format, - f'test_ackermann_drive.xacro.{description_format}' - ) + xacro_processed = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + pkg_share, + description_format, + f'test_ackermann_drive.xacro.{description_format}' + ]), + ] ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -48,11 +49,11 @@ def robot_state_publisher(context): ) return [node_robot_state_publisher] - robot_controllers = os.path.join( + robot_controllers = PathJoinSubstitution([ pkg_share, 'config', 'ackermann_drive_controller.yaml' - ) + ]) joint_state_broadcaster_spawner = Node( package='controller_manager', @@ -92,7 +93,7 @@ def robot_state_publisher(context): # 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'), + config_file=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']), container_name='ros_gz_container', create_own_container='False', use_composition='True', diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index 49e1cf84..58260f07 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -12,16 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -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.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from ros_gz_bridge.actions import RosGzBridge from ros_gz_sim.actions import GzServer -import xacro def generate_launch_description(): @@ -33,12 +30,16 @@ def generate_launch_description(): def robot_state_publisher(context): description_format = LaunchConfiguration('description_format').perform(context) # Get URDF or SDF via xacro - xacro_processed = xacro.process( - os.path.join( - pkg_share, - description_format, - f'test_diff_drive.xacro.{description_format}' - ) + xacro_processed = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + pkg_share, + description_format, + f'test_diff_drive.xacro.{description_format}' + ]), + ] ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -48,11 +49,11 @@ def robot_state_publisher(context): ) return [node_robot_state_publisher] - robot_controllers = os.path.join( + robot_controllers = PathJoinSubstitution([ pkg_share, 'config', 'diff_drive_controller.yaml' - ) + ]) joint_state_broadcaster_spawner = Node( package='controller_manager', @@ -90,7 +91,7 @@ def robot_state_publisher(context): # 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'), + config_file=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']), container_name='ros_gz_container', create_own_container='False', use_composition='True', diff --git a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py index cbef93e8..9a5a21dd 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py @@ -12,16 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -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.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from ros_gz_bridge.actions import RosGzBridge from ros_gz_sim.actions import GzServer -import xacro def generate_launch_description(): @@ -33,13 +30,18 @@ def generate_launch_description(): def robot_state_publisher(context): description_format = LaunchConfiguration('description_format').perform(context) # Get URDF or SDF via xacro - xacro_processed = xacro.process( - os.path.join( - pkg_share, - description_format, - f'test_diff_drive.xacro.{description_format}' - ), - mappings={'namespace': 'r1'} + xacro_processed = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + pkg_share, + description_format, + f'test_diff_drive.xacro.{description_format}' + ]), + ' ', + 'namespace:=r1', + ] ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -50,11 +52,11 @@ def robot_state_publisher(context): ) return [node_robot_state_publisher] - robot_controllers = os.path.join( + robot_controllers = PathJoinSubstitution([ pkg_share, 'config', 'diff_drive_controller.yaml' - ) + ]) joint_state_broadcaster_spawner = Node( package='controller_manager', @@ -98,7 +100,7 @@ def robot_state_publisher(context): # 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'), + config_file=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']), container_name='ros_gz_container', create_own_container='False', use_composition='True', From 9a9a1a3704dba68fc84cbfb3795ed33a74e5201d Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 14:49:53 +0530 Subject: [PATCH 12/13] Fix merge --- .../examples/example_ackermann_drive.cpp | 62 ------------------- .../examples/example_tricycle_drive.cpp | 54 ---------------- 2 files changed, 116 deletions(-) delete mode 100644 gz_ros2_control_demos/examples/example_ackermann_drive.cpp delete mode 100644 gz_ros2_control_demos/examples/example_tricycle_drive.cpp diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp deleted file mode 100644 index 7be1633e..00000000 --- a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2024 Open Source Robotics Foundation, Inc. -// -// 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. - -#include -#include - -#include - -#include - -using namespace std::chrono_literals; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - std::shared_ptr node = - std::make_shared("ackermann_drive_test_node"); - - node->set_parameter(rclcpp::Parameter("use_sim_time", true)); - - auto publisher = node->create_publisher( - "/ackermann_steering_controller/reference", 10); - - RCLCPP_INFO(node->get_logger(), "node created"); - - geometry_msgs::msg::Twist tw; - - tw.linear.x = 0.5; - tw.linear.y = 0.0; - tw.linear.z = 0.0; - - tw.angular.x = 0.0; - tw.angular.y = 0.0; - tw.angular.z = 0.3; - - geometry_msgs::msg::TwistStamped command; - command.twist = tw; - - while (1) { - rclcpp::spin_some(node); - if (node->get_clock()->started()) { - command.header.stamp = node->now(); - publisher->publish(command); - } - std::this_thread::sleep_for(50ms); - } - rclcpp::shutdown(); - - return 0; -} diff --git a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp deleted file mode 100644 index ff49808b..00000000 --- a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2022 Open Source Robotics Foundation, Inc. -// -// 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. - -#include - -#include - -#include - -using namespace std::chrono_literals; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - std::shared_ptr node = - std::make_shared("tricycle_drive_test_node"); - - auto publisher = node->create_publisher( - "/tricycle_controller/cmd_vel", 10); - - RCLCPP_INFO(node->get_logger(), "node created"); - - geometry_msgs::msg::TwistStamped command; - - command.twist.linear.x = 0.2; - command.twist.linear.y = 0.0; - command.twist.linear.z = 0.0; - - command.twist.angular.x = 0.0; - command.twist.angular.y = 0.0; - command.twist.angular.z = 0.1; - - while (1) { - command.header.stamp = node->now(); - publisher->publish(command); - std::this_thread::sleep_for(50ms); - rclcpp::spin_some(node); - } - rclcpp::shutdown(); - - return 0; -} From a6861d7c4a47d68edc783960b25423f6d3900b03 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 27 Mar 2025 14:57:42 +0530 Subject: [PATCH 13/13] Add odometry plugin for mobile_robot demos --- doc/index.rst | 4 ++++ gz_ros2_control_demos/config/ros_gz_bridge_config.yaml | 7 +++++++ gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf | 6 ++++++ gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf | 6 ++++++ gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf | 6 ++++++ gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf | 6 ++++++ 6 files changed, 35 insertions(+) diff --git a/doc/index.rst b/doc/index.rst index ceddc1b2..77e2feab 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -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 ``/odom`` topic to verify how accurate the odometry calculated by the controller is in comparison to the odometry from ``/gz/odom``. + Gripper ----------------------------------------------------------- diff --git a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml index e48c37fb..30766d5f 100644 --- a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml +++ b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml @@ -4,3 +4,10 @@ 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 diff --git a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf index bca986df..dad59a2e 100644 --- a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf @@ -359,6 +359,12 @@ $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml + + + ackermann_drive/odom + ackermann_drive + /gz/odom + diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf index f836b88a..fc3f9b64 100644 --- a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -221,5 +221,11 @@ $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml + + diff_drive/odom + diff_drive + /gz/odom + + diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf index bcdf8205..26f53293 100644 --- a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf @@ -216,6 +216,12 @@ $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml + + + ackermann_drive/odom + ackermann_drive + /gz/odom + diff --git a/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index 08956f33..98c5f75e 100644 --- a/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -176,6 +176,12 @@ $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml + + + diff_drive/odom + diff_drive + /gz/odom +