diff --git a/doc/index.rst b/doc/index.rst index 181480b3..b560e6d7 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -399,3 +399,27 @@ The following examples shows a vertical cart control by a PID joint using positi ros2 run gazebo_ros2_control_demos example_position_pid ros2 run gazebo_ros2_control_demos example_velocity + + +Holonomic Drive(Mecanum Wheels) +----------------------------------------------------------- + +The following example shows mecanum wheels based cart controlled by a `Holonomic_Drive_Controller `__. + +To launch the setup spawning the mecanum wheel based cart, execute the following comand: + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos holonomic_drive.launch.py + +Next to controll the cart excute the following command and read the terminal output on how to control the cart: + +.. code-block:: shell + + ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/mecanum_drive_controller/reference -p stamped:=true + +.. note:: + + Initially the cart would be moving very slowly. Press ``q`` multiple times to increse speed of the cart as per your preference. + + The following `link `__ might help for additional information on how to install the controller. diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 50488e59..96799fab 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -26,6 +26,7 @@ install(DIRECTORY launch config urdf + meshes DESTINATION share/${PROJECT_NAME}/ ) diff --git a/gazebo_ros2_control_demos/config/holonomic_drive_controller.yaml b/gazebo_ros2_control_demos/config/holonomic_drive_controller.yaml new file mode 100644 index 00000000..d1a1324f --- /dev/null +++ b/gazebo_ros2_control_demos/config/holonomic_drive_controller.yaml @@ -0,0 +1,33 @@ +controller_manager: + ros__parameters: + update_rate: 30 + use_sim_time: true + + mecanum_drive_controller: + type: mecanum_drive_controller/MecanumDriveController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + +mecanum_drive_controller: + ros__parameters: + + reference_timeout: 0.0 + + front_left_wheel_command_joint_name: "front_left_joint" + front_right_wheel_command_joint_name: "front_right_joint" + rear_right_wheel_command_joint_name: "rear_right_joint" + rear_left_wheel_command_joint_name: "rear_left_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.056 + sum_of_robot_center_projection_on_X_Y_axis: 0.6 + + base_frame_id: "base_link" + odom_frame_id: "odom" + + enable_odom_tf: true + + use_stamped_vel: true diff --git a/gazebo_ros2_control_demos/launch/holonomic_drive.launch.py b/gazebo_ros2_control_demos/launch/holonomic_drive.launch.py new file mode 100644 index 00000000..c7f421a4 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/holonomic_drive.launch.py @@ -0,0 +1,101 @@ +# Copyright 2020 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. + +# Author: Pratham Pandey + +import os +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.actions import SetEnvironmentVariable +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Process the URDF file + gazebo_ros2_control_demos_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos')) + xacro_file = os.path.join(gazebo_ros2_control_demos_path, + 'urdf', + 'test_holonomic_drive.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + # Robot State Publisher + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + # Gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), + ) + + # Spawn Robot in Gazebo + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'bot'], + output='screen') + + resource_path = SetEnvironmentVariable( + name='GAZEBO_MODEL_PATH', + value=[str(Path(gazebo_ros2_control_demos_path).parent.resolve())]) + + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_mecanum_drive_controller_base_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'mecanum_drive_controller'], + output='screen' + ) + + # Launch them all! + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_mecanum_drive_controller_base_controller], + ) + ), + resource_path, + node_robot_state_publisher, + gazebo, + spawn_entity, + ]) + diff --git a/gazebo_ros2_control_demos/meshes/roller.stl b/gazebo_ros2_control_demos/meshes/roller.stl new file mode 100644 index 00000000..1e7cc6ef Binary files /dev/null and b/gazebo_ros2_control_demos/meshes/roller.stl differ diff --git a/gazebo_ros2_control_demos/urdf/test_holonomic_drive.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_holonomic_drive.xacro.urdf new file mode 100644 index 00000000..94b54fba --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_holonomic_drive.xacro.urdf @@ -0,0 +1,459 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${wheel_color} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Yellow + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Purple + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + -10 + 10 + + + + + + + + -10 + 10 + + + + + + + + -10 + 10 + + + + + + + + -10 + 10 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find gazebo_ros2_control_demos)/config/holonomic_drive_controller.yaml + + ~/tf_odometry:=/tf + + + + +