Added Gazebo Demo for Mecanum Drive Controller

doc/index.rst

@@ -399,3 +399,31 @@ 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 <https://github.com/ros-controls/ros2_controllers/pull/512>`__. 
+
+
+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:=/mec_cont/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 <https://robotics.stackexchange.com/questions/111154/holonomic-drive-in-ros2-using-mecanum-wheels>`__ might help for additional information on how to install the controller.

gazebo_ros2_control_demos/config/holonomic_drive_controller.yaml

@@ -0,0 +1,31 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 30
+    use_sim_time: true
+
+    mec_cont:
+      type: mecanum_drive_controller/MecanumDriveController
+
+    joint_broad:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+mec_cont:
+  ros__parameters:
+
+    publish_rate: 30.0 
+
+    command_joint_names: ["front_left_joint", "rear_left_joint", "rear_right_joint", "front_right_joint"]                
+
+    interface_name: velocity    
+
+    kinematics:
+        wheels_radius: 0.4 
+        sum_of_robot_center_projection_on_X_Y_axis: 1.0       
+
+    base_frame_id: "base_link"
+    odom_frame_id: "odom" 
+
+    enable_odom_tf: true 
+
+    use_stamped_vel: true              

gazebo_ros2_control_demos/launch/holonomic_drive.launch.py

@@ -0,0 +1,73 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    package_name='gazebo_ros2_control_demos' 
+
+    # Process the URDF file
+    pkg_path = os.path.join(get_package_share_directory(package_name))
+    xacro_file = os.path.join(pkg_path, 'urdf', 'test_holonomic_drive.xacro.urdf')
+    robot_description_config = xacro.process_file(xacro_file)
+
+    # Robot State Publisher
+    params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': True}
+    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')    
+
+    # Holonomic Drive Conroller
+    mec_cont_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["mec_cont"],
+    )
+
+    # Joint State Broadcaster
+    joint_broad_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_broad"],
+    )
+
+    # RVIZ
+    rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen'
+        )       
+
+    # Launch them all!
+    return LaunchDescription([               
+        node_robot_state_publisher,
+        gazebo,
+        spawn_entity,        
+        mec_cont_spawner,
+        joint_broad_spawner,       
+        rviz
+    ])
+