PID controller and diff drive controller chaining

README.md

@@ -78,6 +78,10 @@ The following examples are part of this demo repository:
 
    This example shows how to integrate multiple robots under different controller manager instances.
 
+* Example 16: ["DiffBot with Chained Controllers"](example_16)
+
+   This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot.
+
 ## Structure
 
 The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`.

doc/index.rst

@@ -85,6 +85,8 @@ Example 14: "Modular robots with actuators not providing states and with additio
 Example 15: "Using multiple controller managers"
    This example shows how to integrate multiple robots under different controller manager instances.
 
+Example 16: "DiffBot with chained controllers"
+   This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot.
 
 .. _ros2_control_demos_install:
 
@@ -280,3 +282,4 @@ Examples
    Example 13: Multiple robots <../example_13/doc/userdoc.rst>
    Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst>
    Example 15: Using multiple controller managers <../example_15/doc/userdoc.rst>
+   Example 16: DiffBot with chained controllers <../example_16/doc/userdoc.rst>

example_16/CMakeLists.txt

@@ -0,0 +1,84 @@
+cmake_minimum_required(VERSION 3.16)
+project(ros2_control_demo_example_16 LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+# set the same behavior for windows as it is on linux
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+# Specify the required version of ros2_control
+find_package(controller_manager 4.0.0)
+# Handle the case where the required version is not found
+if(NOT controller_manager_FOUND)
+  message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. "
+  "Are you using the correct branch of the ros2_control_demos repository?")
+endif()
+
+# find dependencies
+find_package(backward_ros REQUIRED)
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+## COMPILE
+add_library(
+  ros2_control_demo_example_16
+  SHARED
+  hardware/diffbot_system.cpp
+)
+target_compile_features(ros2_control_demo_example_16 PUBLIC cxx_std_17)
+target_include_directories(ros2_control_demo_example_16 PUBLIC
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+$<INSTALL_INTERFACE:include/ros2_control_demo_example_16>
+)
+ament_target_dependencies(
+  ros2_control_demo_example_16 PUBLIC
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_16.xml)
+
+# INSTALL
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include/ros2_control_demo_example_16
+)
+install(
+  DIRECTORY description/launch description/ros2_control description/urdf
+  DESTINATION share/ros2_control_demo_example_16
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/ros2_control_demo_example_16
+)
+install(TARGETS ros2_control_demo_example_16
+  EXPORT export_ros2_control_demo_example_16
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  ament_add_pytest_test(example_16_urdf_xacro test/test_urdf_xacro.py)
+  ament_add_pytest_test(view_example_16_launch test/test_view_robot_launch.py)
+  ament_add_pytest_test(run_example_16_launch test/test_diffbot_launch.py)
+endif()
+
+## EXPORTS
+ament_export_targets(export_ros2_control_demo_example_16 HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

example_16/README.md

@@ -0,0 +1,6 @@
+# ros2_control_demo_example_16
+
+   *DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive.
+   The robot is basically a box moving according to differential drive kinematics.
+
+Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_16/doc/userdoc.html).

example_16/bringup/config/diffbot_chained_controllers.yaml

@@ -0,0 +1,92 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    pid_controller_left_wheel_joint:
+      type: pid_controller/PidController
+
+    pid_controller_right_wheel_joint:
+      type: pid_controller/PidController
+
+    diffbot_base_controller:
+      type: diff_drive_controller/DiffDriveController
+
+
+pid_controller_left_wheel_joint:
+  ros__parameters:
+
+    dof_names:
+      - left_wheel_joint
+
+    command_interface: velocity
+
+    reference_and_state_interfaces:
+      - velocity
+
+    gains:
+      # control the velocity, no d term
+      left_wheel_joint: {"p": 0.5, "i": 2.5, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95}
+
+pid_controller_right_wheel_joint:
+  ros__parameters:
+
+    dof_names:
+      - right_wheel_joint
+
+    command_interface: velocity
+
+    reference_and_state_interfaces:
+      - velocity
+
+    gains:
+      # control the velocity, no d term
+      right_wheel_joint: {"p": 0.5, "i": 2.5, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95}
+
+diffbot_base_controller:
+  ros__parameters:
+
+    left_wheel_names: ["pid_controller_left_wheel_joint/left_wheel_joint"]
+    right_wheel_names: ["pid_controller_right_wheel_joint/right_wheel_joint"]
+
+    wheel_separation: 0.10
+    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.015
+
+    # we have velocity feedback
+    position_feedback: false
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_link
+    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: true
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    # set publish_limited_velocity to true for visualization
+    publish_limited_velocity: true
+    #velocity_rolling_window_size: 10
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: .NAN
+    linear.x.min_jerk: .NAN
+
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: .NAN
+    angular.z.min_jerk: .NAN

example_16/bringup/config/plotjuggler.xml

@@ -0,0 +1,84 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab containers="1" tab_name="DiffBot">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.300292;0.300292;0.399417" count="3">
+     <DockArea name="...">
+      <plot flip_y="false" mode="TimeSeries" flip_x="false" style="LinesAndDots">
+       <range top="0.717500" bottom="-0.017500" right="10.379761" left="2.746329"/>
+       <limitY/>
+       <curve name="/cmd_vel/twist/linear/x" color="#f1854c"/>
+       <curve name="/diffbot_base_controller/cmd_vel_out/twist/linear/x" color="#d62728"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
+       <range top="1.025000" bottom="-0.025000" right="10.379761" left="2.746329"/>
+       <limitY/>
+       <curve name="/cmd_vel/twist/angular/z" color="#17becf"/>
+       <curve name="/diffbot_base_controller/cmd_vel_out/twist/angular/z" color="#bcbd22"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_y="false" mode="TimeSeries" flip_x="false" style="LinesAndDots">
+       <range top="52.434211" bottom="-0.855263" right="10.379761" left="2.746329"/>
+       <limitY/>
+       <curve name="/controller_manager/introspection_data/values/state_interface.left_wheel_joint/velocity" color="#1f77b4"/>
+       <curve name="/controller_manager/introspection_data/values/command_interface.left_wheel_joint/velocity" color="#d62728"/>
+       <curve name="/controller_manager/introspection_data/values/command_interface.pid_controller_left_wheel_joint/left_wheel_joint/velocity" color="#1ac938"/>
+       <curve name="/controller_manager/introspection_data/values/state_interface.right_wheel_joint/velocity" color="#1ac9bc"/>
+       <curve name="/controller_manager/introspection_data/values/command_interface.right_wheel_joint/velocity" color="#ff7f0e"/>
+       <curve name="/controller_manager/introspection_data/values/command_interface.pid_controller_right_wheel_joint/right_wheel_joint/velocity" color="#f14cc1"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <parameters delimiter="0" time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad ROS2 bags">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+   <boolean_strings_to_number value="true"/>
+   <remove_suffix_from_strings value="true"/>
+   <selected_topics value=""/>
+  </plugin>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="ROS2 Topic Subscriber">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+   <boolean_strings_to_number value="true"/>
+   <remove_suffix_from_strings value="true"/>
+   <selected_topics value="/cmd_vel;/controller_manager/introspection_data/names;/controller_manager/introspection_data/values;/diffbot_base_controller/cmd_vel_out"/>
+  </plugin>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indices, not Lua indices&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+  <plugin ID="ROS2 Topic Re-Publisher"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>

example_16/bringup/launch/demo_test.launch.py

@@ -0,0 +1,40 @@
+# Copyright 2025 ros2_control Development Team
+#
+# 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.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import ExecuteProcess
+
+
+def generate_launch_description():
+
+    return LaunchDescription(
+        [
+            ExecuteProcess(
+                cmd=[
+                    "python3",
+                    PathJoinSubstitution(
+                        [
+                            FindPackageShare("ros2_control_demo_example_16"),
+                            "launch",
+                            "demo_test_helper.py",
+                        ]
+                    ),
+                ],
+                output="screen",
+            )
+        ]
+    )