Added Gazebo Demo for Mecanum Drive Controller

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 <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:=/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 <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/CMakeLists.txt

@@ -26,6 +26,7 @@ install(DIRECTORY
   launch
   config
   urdf
+  meshes
   DESTINATION share/${PROJECT_NAME}/
 )
 

gazebo_ros2_control_demos/config/holonomic_drive_controller.yaml

@@ -0,0 +1,31 @@
+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:
+
+    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,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,
+    ])
+