From fca7e00aaf5ffb862f326dc579688879a6a6b577 Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Wed, 11 Dec 2024 09:44:13 -0500 Subject: [PATCH] Add Mecanum vehicle example (#451) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero (cherry picked from commit 18bdde12b46814d9b4607817a7f5df0cb0930364) --- doc/index.rst | 7 + .../config/mecanum_drive_controller.yaml | 25 ++ .../launch/mecanum_drive_example.launch.py | 116 ++++++++ gz_ros2_control_demos/package.xml | 1 + .../urdf/test_mecanum_drive.xacro.urdf | 270 ++++++++++++++++++ 5 files changed, 419 insertions(+) create mode 100644 gz_ros2_control_demos/config/mecanum_drive_controller.yaml create mode 100644 gz_ros2_control_demos/launch/mecanum_drive_example.launch.py create mode 100644 gz_ros2_control_demos/urdf/test_mecanum_drive.xacro.urdf diff --git a/doc/index.rst b/doc/index.rst index b8401f8f..13db3e1e 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -280,6 +280,13 @@ 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 +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 ----------------------------------------------------------- diff --git a/gz_ros2_control_demos/config/mecanum_drive_controller.yaml b/gz_ros2_control_demos/config/mecanum_drive_controller.yaml new file mode 100644 index 00000000..1c85486a --- /dev/null +++ b/gz_ros2_control_demos/config/mecanum_drive_controller.yaml @@ -0,0 +1,25 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + use_sim_time: true + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + mecanum_drive_controller: + type: mecanum_drive_controller/MecanumDriveController + +mecanum_drive_controller: + ros__parameters: + reference_timeout: 1.0 # max time between command messages before the vehicle should stop + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "rear_right_wheel_joint" + rear_left_wheel_command_joint_name: "rear_left_wheel_joint" + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.3 + sum_of_robot_center_projection_on_X_Y_axis: 1.1 + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true diff --git a/gz_ros2_control_demos/launch/mecanum_drive_example.launch.py b/gz_ros2_control_demos/launch/mecanum_drive_example.launch.py new file mode 100644 index 00000000..9bc08170 --- /dev/null +++ b/gz_ros2_control_demos/launch/mecanum_drive_example.launch.py @@ -0,0 +1,116 @@ +# 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. + +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_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # 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_mecanum_drive.xacro.urdf'] + ), + ] + ) + robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'mecanum_drive_controller.yaml', + ] + ) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', + '-name', 'mecanum_vehicle', '-allow_renaming', 'true'], + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], + ) + mecanum_drive_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'mecanum_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' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py'])]), + launch_arguments=[('gz_args', [' -r -v 4 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=[mecanum_drive_controller_spawner], + ) + ), + bridge, + node_robot_state_publisher, + gz_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index 9f7b13f6..6700d367 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -44,6 +44,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + mecanum_drive_controller ros2controlcli tricycle_controller velocity_controllers diff --git a/gz_ros2_control_demos/urdf/test_mecanum_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_mecanum_drive.xacro.urdf new file mode 100644 index 00000000..2b13afdc --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_mecanum_drive.xacro.urdf @@ -0,0 +1,270 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + + + + $(find gz_ros2_control_demos)/config/mecanum_drive_controller.yaml + + /mecanum_drive_controller/reference:=/cmd_vel + /mecanum_drive_controller/odom:=/odom + + + + +