Adds diffdrive controller (iRobotEducation#4)

* diffdrive control package

* removes unnecessary dependency

* Adds missing dependencies

* code cleanup

* comment hard to understand code

* address review comments

* updates control.yaml

* included into irobot_create_gazebo and launched from there

* fix flake 8 erros

* applies 'safety enabled' controller parameters to robot

* uses joint_state_publisher instead of joint_state_publisher_gui

* Address comments

* doubled the cmd_vel_timeout

* republish node

* Address comments

* Adds QoS parameter

* Add resource directory

* creates irobot_create_toolbox package

* changes order of node execution

* same name convention to variables

* correct flake8 errors

* more flake 8 fixes

* new line at end of gitignore

* flake8

* flake8

* replace subscription by underscore

* rename QoS to qos

* add dependency rcl_interfaces

* add test_depends

* get qos and message type by inspecting publisher

* adds uncrustify

* fix flake 8

* updates .gitignore

* check_published_topic moved to init

* Adds the correct version of uncrustify and makes republisher output debug.

* Fix merge conflicts

* Fix uncrustify failures

* Update irobot_create_toolbox/irobot_create_toolbox/topic_republisher.py

Co-authored-by: Emiliano Borghi <creativa_eborghi@irobot.com>

* Add comments to explain functionality

* removes uncrustify because collides with clang_format

* Obtain qos and msg_type from subscriber

* Adds docstrings

* Subscription instead of subscriber

* docstring how to use the tool

Co-authored-by: Emiliano Borghi <creativa_eborghi@irobot.com>

.github/workflows/ci.yml

@@ -21,11 +21,13 @@ jobs:
           target-ros2-distro: foxy
           import-token: ${{ secrets.REPO_TOKEN }}
           package-name:
-            irobot_create_msgs
+            irobot_create_control
             irobot_create_description
             irobot_create_gazebo
             irobot_create_gazebo_plugins
+            irobot_create_msgs
             irobot_create_sim
+            irobot_create_toolbox
 
       # ============
       # DEPENDENCIES

.github/workflows/lint.yml

@@ -36,8 +36,10 @@ jobs:
           distribution: foxy
           linter: ${{ matrix.linter }}
           package-name:
-            irobot_create_msgs
+            irobot_create_control
             irobot_create_description
             irobot_create_gazebo
             irobot_create_gazebo_plugins
+            irobot_create_msgs
             irobot_create_sim
+            irobot_create_toolbox

.gitignore

@@ -1 +1,2 @@
 .vscode
+__pycache__

irobot_create_control/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(irobot_create_control)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    launch
+    config
+  DESTINATION
+    share/${PROJECT_NAME}/
+)
+
+ament_package()

irobot_create_control/config/control.yaml

@@ -0,0 +1,71 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    diffdrive_controller:
+      type: diff_drive_controller/DiffDriveController
+
+diffdrive_controller:
+  ros__parameters:
+    left_wheel_names: ["wheel_left_joint"]
+    right_wheel_names: ["wheel_right_joint"]
+
+    wheel_separation: 0.233
+    wheel_radius: 0.03575
+
+    # Multiplier applied to the wheel separation parameter.
+    # This is used to account for a difference between the robot model and a real robot.
+    wheel_separation_multiplier: 1.0
+
+    # Multipliers applied to the wheel radius parameter.
+    # This is used to account for a difference between the robot model and a real robot.
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 62.0
+    odom_frame_id: odom
+    base_frame_id: base_footprint
+    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+    open_loop: false
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    use_stamped_vel: false
+
+    # Preserve turning radius when limiting speed/acceleration/jerk
+    preserve_turning_radius: true
+
+    # Publish limited velocity
+    publish_cmd: true
+
+    # Publish wheel data
+    publish_wheel_data: true
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 0.306
+    linear.x.min_velocity: -0.306
+    linear.x.max_acceleration: 0.9
+    # Not using jerk limits yet
+    # linear.x.max_jerk: 0.0
+    # linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: false
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 1.9
+    angular.z.min_velocity: -1.9
+    # Not using angular acceleration limits yet
+    # angular.z.max_acceleration: 1.0
+    # angular.z.min_acceleration: -1.0
+    # Not using jerk limits yet
+    # angular.z.max_jerk: 0.0
+    # angular.z.min_jerk: 0.0

irobot_create_control/launch/control.launch.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+# Copyright 2021 iRobot Corporation. All Rights Reserved.
+# 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 Rodrigo Jose Causarano Nunez (rcausaran@irobot.com)
+#
+# Launch Create3 with diffdrive controller in Gazebo and optionally also in RViz.
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_create3_control = get_package_share_directory('irobot_create_control')
+
+    control_params_file = PathJoinSubstitution(
+        [pkg_create3_control, 'config', 'control.yaml'])
+
+    diffdrive_controller_node = Node(
+        package='controller_manager',
+        executable='spawner.py',
+        parameters=[control_params_file],
+        arguments=['diffdrive_controller', '-c', '/controller_manager'],
+        output='screen',
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner.py',
+        arguments=['joint_state_broadcaster', '-c', '/controller_manager'],
+        output='screen',
+    )
+
+    cmd_vel_republish_node = Node(
+        package='irobot_create_toolbox',
+        name='republish_node',
+        executable='topic_republisher',
+        parameters=[
+            {'current_topic': 'cmd_vel'},
+            {'new_topic': 'diffdrive_controller/cmd_vel_unstamped'},
+        ]
+    )
+
+    # Ensure diffdrive_controller_node starts after joint_state_broadcaster_spawner
+    diffdrive_controller_callback = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[diffdrive_controller_node],
+        )
+    )
+
+    ld = LaunchDescription()
+
+    ld.add_action(joint_state_broadcaster_spawner)
+    ld.add_action(diffdrive_controller_callback)
+    ld.add_action(cmd_vel_republish_node)
+
+    return ld

irobot_create_control/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>irobot_create_control</name>
+  <version>0.0.1</version>
+  <description>Provides the diff-drive controller for the iRobot Create 3 platform.</description>
+  <maintainer email="ekumen@irbt.onmicrosoft.com">Ekumen</maintainer>
+  <license>Apache-2.0</license>
+
+  <author>iRobot</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>gazebo_ros2_control</exec_depend>
+  <exec_depend>joint_state_controller</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+  <exec_depend>irobot_create_toolbox</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

irobot_create_description/launch/rviz2.launch.py

@@ -41,7 +41,6 @@ def generate_launch_description():
         name='rviz2',
         arguments=['-d', rviz_config_dir],
         condition=IfCondition(LaunchConfiguration('rviz')),
-        output='screen',
     )
 
     robot_state_publisher = Node(
@@ -56,10 +55,9 @@ def generate_launch_description():
     )
 
     joint_state_publisher = Node(
-        package='joint_state_publisher_gui',
-        executable='joint_state_publisher_gui',
-        name='joint_state_publisher_gui',
-        condition=IfCondition(LaunchConfiguration('rviz')),
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
         output='screen',
     )
 

irobot_create_description/package.xml

@@ -14,8 +14,10 @@
   <depend>urdf</depend>
   <depend>xacro</depend>
 
-  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros2launch</exec_depend>
+  <exec_depend>irobot_create_control</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

irobot_create_description/rviz/model.rviz

@@ -10,7 +10,7 @@ Panels:
         - /RobotModel1
         - /RobotModel1/Description Topic1
       Splitter Ratio: 0.5
-    Tree Height: 623
+    Tree Height: 617
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -76,25 +76,8 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        caster_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
           Value: true
-        wheel_left_axle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        wheel_left_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wheel_right_axle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        wheel_right_link:
+        front_caster_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -107,7 +90,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_footprint
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -173,7 +156,7 @@ Window Geometry:
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002f8000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002f8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:

irobot_create_description/urdf/create3.urdf.xacro

@@ -86,4 +86,10 @@
       rpy="${-pi/2} 0 0"/>
   </xacro:caster>
 
+  <gazebo>
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+      <parameters> $(find irobot_create_control)/config/control.yaml </parameters>
+    </plugin>
+  </gazebo>
+
 </robot>

irobot_create_description/urdf/wheel.urdf.xacro

@@ -39,5 +39,15 @@
     <material>Gazebo/DarkGrey</material>
   </gazebo>
 
+  <ros2_control name="${name}_wheel_system" type="system">
+      <hardware>
+        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      </hardware>
+
+      <joint name="wheel_${name}_joint">
+        <command_interface name="velocity" />
+      </joint>
+  </ros2_control>
+
 </xacro:macro>
 </robot>

irobot_create_gazebo/launch/create3.launch.py

@@ -42,8 +42,12 @@
 
 def generate_launch_description():
     # Directories
+    pkg_create3_control = get_package_share_directory('irobot_create_control')
     pkg_create3_description = get_package_share_directory('irobot_create_description')
+
     # Paths
+    control_launch_file = PathJoinSubstitution(
+        [pkg_create3_control, 'launch', 'control.launch.py'])
     description_launch_file = PathJoinSubstitution(
         [pkg_create3_description, 'launch', 'rviz2.launch.py'])
     dock_launch_file = PathJoinSubstitution(
@@ -57,6 +61,9 @@ def generate_launch_description():
     robot_description = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([description_launch_file])
     )
+    diffdrive_controller = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([control_launch_file])
+    )
     spawn_dock = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([dock_launch_file]),
         condition=IfCondition(LaunchConfiguration('dock')),
@@ -99,6 +106,7 @@ def generate_launch_description():
     ld = LaunchDescription(ARGUMENTS)
     # Include robot description
     ld.add_action(robot_description)
+    ld.add_action(diffdrive_controller)
     # Add nodes to LaunchDescription
     ld.add_action(gzserver)
     ld.add_action(gzclient)

irobot_create_gazebo/package.xml

@@ -14,8 +14,10 @@
   <depend>urdf</depend>
   <depend>xacro</depend>
 
+  <exec_depend>irobot_create_control</exec_depend>
   <exec_depend>irobot_create_description</exec_depend>
   <exec_depend>irobot_create_gazebo_plugins</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
   <exec_depend>ros2launch</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>