Added example_17: chained controllers for diff drive using effort interface and Gazebo.

README.md

@@ -85,6 +85,10 @@ The following examples are part of this demo repository:
 
    This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot.
 
+* Example 17: ["DiffBot with Chained Controllers using effort interface"](example_17)
+
+   This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot using effort interface.
+
 ## Structure
 
 The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`.

example_17/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.16)
+project(ros2_control_demo_example_17)
+
+# INSTALL
+install(
+  DIRECTORY description/gazebo description/launch description/ros2_control description/urdf
+  DESTINATION share/ros2_control_demo_example_17
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/ros2_control_demo_example_17
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  ament_add_pytest_test(example_17_urdf_xacro test/test_urdf_xacro.py)
+  ament_add_pytest_test(view_example_17_launch test/test_view_robot_launch.py)
+  ament_add_pytest_test(run_example_17_launch test/test_diffbot_launch.py)
+endif()
+
+ament_package()

example_17/README.md

@@ -0,0 +1,5 @@
+# ros2_control_demo_example_17
+
+   *DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive. This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot using effort interface.
+
+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_17/doc/userdoc.html).

example_17/bringup/config/diffbot_chained_controllers.yaml

@@ -0,0 +1,85 @@
+controller_manager:
+  ros__parameters:
+    use_sim_time: True  # remove on real robots
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    wheel_pids:
+      type: pid_controller/PidController
+
+    diffbot_base_controller:
+      type: diff_drive_controller/DiffDriveController
+
+joint_state_broadcaster:
+  ros__parameters:
+    use_sim_time: True  # remove on real robots
+    update_rate: 100  # Hz
+
+wheel_pids:
+  ros__parameters:
+    use_sim_time: True  # remove on real robots
+    update_rate: 100  # Hz
+
+    dof_names:
+      - left_wheel_joint
+      - right_wheel_joint
+
+    command_interface: effort
+
+    reference_and_state_interfaces:
+      - velocity
+      - effort
+
+    gains:
+      # control the velocity through effort
+      left_wheel_joint: {"p": 0.1, "i": 1.0, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95}
+      right_wheel_joint: {"p": 0.1, "i": 1.0, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95}
+
+diffbot_base_controller:
+  ros__parameters:
+    use_sim_time: True  # remove on real robots
+    update_rate: 100  # Hz
+
+    left_wheel_names: ["wheel_pids/left_wheel_joint"]
+    right_wheel_names: ["wheel_pids/right_wheel_joint"]
+
+    wheel_separation: 0.10
+    wheel_radius: 0.015
+
+    # we have velocity feedback
+    position_feedback: false
+
+    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: false
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    publish_limited_velocity: 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: 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.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    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_17/bringup/config/plotjuggler.xml

@@ -0,0 +1,72 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="tab1" containers="1">
+   <Container>
+    <DockSplitter sizes="0.5;0.5" orientation="-" count="2">
+     <DockArea name="...">
+      <plot mode="TimeSeries" flip_x="false" style="Lines" flip_y="false">
+       <range bottom="-1.251452" top="51.307839" left="1744856394.413297" right="1744856681.372036"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/controller_manager/introspection_data/values/state_interface.left_wheel_joint/velocity"/>
+       <curve color="#d62728" name="/controller_manager/introspection_data/values/state_interface.right_wheel_joint/velocity"/>
+       <curve color="#1ac938" name="/controller_manager/introspection_data/values/command_interface.wheel_pids/left_wheel_joint/velocity"/>
+       <curve color="#ff7f0e" name="/controller_manager/introspection_data/values/command_interface.wheel_pids/right_wheel_joint/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot mode="TimeSeries" flip_x="false" style="Lines" flip_y="false">
+       <range bottom="-0.303565" top="10.336151" left="1744856394.413297" right="1744856681.372036"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/controller_manager/introspection_data/values/command_interface.left_wheel_joint/effort"/>
+       <curve color="#9467bd" name="/controller_manager/introspection_data/values/command_interface.right_wheel_joint/effort"/>
+       <curve color="#17becf" name="/controller_manager/introspection_data/values/state_interface.left_wheel_joint/effort"/>
+       <curve color="#bcbd22" name="/controller_manager/introspection_data/values/state_interface.right_wheel_joint/effort"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="0"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <parameters time_axis="" delimiter="0"/>
+  </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;/diffbot_base_controller/odom;/dynamic_joint_states;/gains/left_wheel_joint/pid_state;/gains/right_wheel_joint/pid_state;/joint_states;/tf;/tf_static;/wheel_pids/controller_state;/wheel_pids/reference"/>
+  </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>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+</root>

example_17/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_17"),
+                            "launch",
+                            "demo_test_helper.py",
+                        ]
+                    ),
+                ],
+                output="screen",
+            )
+        ]
+    )

example_17/bringup/launch/demo_test_helper.py

@@ -0,0 +1,64 @@
+# 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.
+
+import time
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TwistStamped
+from std_srvs.srv import SetBool
+
+
+class DiffbotChainedControllersTest(Node):
+    def __init__(self):
+        super().__init__("diffbot_chained_controllers_demo_helper_node")
+        # Enable feedforward control via service call
+        self.client_ = self.create_client(SetBool, "/wheel_pids/set_feedforward_control")
+        self.publisher_ = self.create_publisher(TwistStamped, "/cmd_vel", 10)
+
+    def set_feedforward_control(self):
+        while not self.client_.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Waiting for feedforward control service to be available...")
+
+        request = SetBool.Request()
+        request.data = True
+        future = self.client_.call_async(request)
+
+        rclpy.spin_until_future_complete(self, future)
+
+        self.get_logger().info("Enabled feedforward control for both wheels.")
+
+    def publish_cmd_vel(self, delay=0.1):
+
+        twist_msg = TwistStamped()
+        twist_msg.twist.linear.x = 0.7
+        twist_msg.twist.linear.y = 0.0
+        twist_msg.twist.linear.z = 0.0
+        twist_msg.twist.angular.x = 0.0
+        twist_msg.twist.angular.y = 0.0
+        twist_msg.twist.angular.z = 1.0
+
+        while rclpy.ok():
+            self.get_logger().info(f"Publishing twist message to cmd_vel: {twist_msg}")
+            self.publisher_.publish(twist_msg)
+            time.sleep(delay)
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    test_node = DiffbotChainedControllersTest()
+    test_node.set_feedforward_control()
+    test_node.publish_cmd_vel(delay=0.1)
+    rclpy.spin(test_node)
+    test_node.destroy_node()
+    rclpy.shutdown()