PID controller and diff drive controller chaining

.gitignore

@@ -1,2 +1,4 @@
 .ccache
 .work
+/.vscode
+*.pyc

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`.

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,101 @@
+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
+
+    forward_velocity_controller_for_debug:
+      type: forward_command_controller/ForwardCommandController
+
+
+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.15, "i": 0.05, "d": 0.0, "i_clamp_min": -2.0, "i_clamp_max": 2.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.15, "i": 0.05, "d": 0.0, "i_clamp_min": -2.0, "i_clamp_max": 2.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
+    #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
+
+forward_velocity_controller_for_debug:
+  ros__parameters:
+    joints:
+      - pid_controller_left_wheel_joint/left_wheel_joint
+      - pid_controller_right_wheel_joint/right_wheel_joint
+    interface_name: velocity

example_16/bringup/launch/diffbot.launch.py

@@ -0,0 +1,176 @@
+# 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.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    PathJoinSubstitution,
+    LaunchConfiguration,
+)
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_mock_hardware",
+            default_value="false",
+            description="Start robot with mock hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fixed_frame_id",
+            default_value="odom",
+            description="Fixed frame id of the robot.",
+        )
+    )
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
+    fixed_frame_id = LaunchConfiguration("fixed_frame_id")
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("ros2_control_demo_example_16"), "urdf", "diffbot.urdf.xacro"]
+            ),
+            " ",
+            "use_mock_hardware:=",
+            use_mock_hardware,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_example_16"),
+            "config",
+            "diffbot_chained_controllers.yaml",
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+    )
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file, "-f", fixed_frame_id],
+        condition=IfCondition(gui),
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+    )
+
+    pid_controllers_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "pid_controller_left_wheel_joint",
+            "pid_controller_right_wheel_joint",
+            "--param-file",
+            robot_controllers,
+        ],
+    )
+
+    # start the base controller in inactive mode
+    robot_base_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "diffbot_base_controller",
+            "--param-file",
+            robot_controllers,
+            "--controller-ros-args",
+            "-r /diffbot_base_controller/cmd_vel:=/cmd_vel",
+            # "--inactive",
+            "--ros-args",
+            "--log-level",
+            "debug",
+        ],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    delay_robot_base_after_pid_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=pid_controllers_spawner,
+            on_exit=[robot_base_controller_spawner],
+        )
+    )
+
+    # Delay start of joint_state_broadcaster after `robot_base_controller`
+    # TODO(anyone): This is a workaround for flaky tests. Remove when fixed.
+    delay_joint_state_broadcaster_after_robot_base_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=robot_base_controller_spawner,
+            on_exit=[joint_state_broadcaster_spawner],
+        )
+    )
+
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        pid_controllers_spawner,
+        delay_robot_base_after_pid_controller_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_joint_state_broadcaster_after_robot_base_controller_spawner,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)