Skip to content

Commit c12d5b2

Browse files
PID controller and diff drive controller chaining (#710)
Co-authored-by: Christoph Froehlich <[email protected]>
1 parent 596674c commit c12d5b2

21 files changed

+1545
-0
lines changed

README.md

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,10 @@ The following examples are part of this demo repository:
7878

7979
This example shows how to integrate multiple robots under different controller manager instances.
8080

81+
* Example 16: ["DiffBot with Chained Controllers"](example_16)
82+
83+
This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot.
84+
8185
## Structure
8286

8387
The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`.

doc/index.rst

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,8 @@ Example 14: "Modular robots with actuators not providing states and with additio
8585
Example 15: "Using multiple controller managers"
8686
This example shows how to integrate multiple robots under different controller manager instances.
8787

88+
Example 16: "DiffBot with chained controllers"
89+
This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot.
8890

8991
.. _ros2_control_demos_install:
9092

@@ -280,3 +282,4 @@ Examples
280282
Example 13: Multiple robots <../example_13/doc/userdoc.rst>
281283
Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst>
282284
Example 15: Using multiple controller managers <../example_15/doc/userdoc.rst>
285+
Example 16: DiffBot with chained controllers <../example_16/doc/userdoc.rst>

example_16/CMakeLists.txt

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(ros2_control_demo_example_16 LANGUAGES CXX)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra)
6+
endif()
7+
8+
# set the same behavior for windows as it is on linux
9+
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
10+
11+
# find dependencies
12+
set(THIS_PACKAGE_INCLUDE_DEPENDS
13+
hardware_interface
14+
pluginlib
15+
rclcpp
16+
rclcpp_lifecycle
17+
)
18+
19+
# Specify the required version of ros2_control
20+
find_package(controller_manager 4.0.0)
21+
# Handle the case where the required version is not found
22+
if(NOT controller_manager_FOUND)
23+
message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. "
24+
"Are you using the correct branch of the ros2_control_demos repository?")
25+
endif()
26+
27+
# find dependencies
28+
find_package(backward_ros REQUIRED)
29+
find_package(ament_cmake REQUIRED)
30+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
31+
find_package(${Dependency} REQUIRED)
32+
endforeach()
33+
34+
## COMPILE
35+
add_library(
36+
ros2_control_demo_example_16
37+
SHARED
38+
hardware/diffbot_system.cpp
39+
)
40+
target_compile_features(ros2_control_demo_example_16 PUBLIC cxx_std_17)
41+
target_include_directories(ros2_control_demo_example_16 PUBLIC
42+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
43+
$<INSTALL_INTERFACE:include/ros2_control_demo_example_16>
44+
)
45+
ament_target_dependencies(
46+
ros2_control_demo_example_16 PUBLIC
47+
${THIS_PACKAGE_INCLUDE_DEPENDS}
48+
)
49+
50+
# Export hardware plugins
51+
pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_16.xml)
52+
53+
# INSTALL
54+
install(
55+
DIRECTORY hardware/include/
56+
DESTINATION include/ros2_control_demo_example_16
57+
)
58+
install(
59+
DIRECTORY description/launch description/ros2_control description/urdf
60+
DESTINATION share/ros2_control_demo_example_16
61+
)
62+
install(
63+
DIRECTORY bringup/launch bringup/config
64+
DESTINATION share/ros2_control_demo_example_16
65+
)
66+
install(TARGETS ros2_control_demo_example_16
67+
EXPORT export_ros2_control_demo_example_16
68+
ARCHIVE DESTINATION lib
69+
LIBRARY DESTINATION lib
70+
RUNTIME DESTINATION bin
71+
)
72+
73+
if(BUILD_TESTING)
74+
find_package(ament_cmake_pytest REQUIRED)
75+
76+
ament_add_pytest_test(example_16_urdf_xacro test/test_urdf_xacro.py)
77+
ament_add_pytest_test(view_example_16_launch test/test_view_robot_launch.py)
78+
ament_add_pytest_test(run_example_16_launch test/test_diffbot_launch.py)
79+
endif()
80+
81+
## EXPORTS
82+
ament_export_targets(export_ros2_control_demo_example_16 HAS_LIBRARY_TARGET)
83+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
84+
ament_package()

example_16/README.md

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# ros2_control_demo_example_16
2+
3+
*DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive.
4+
The robot is basically a box moving according to differential drive kinematics.
5+
6+
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).
Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
controller_manager:
2+
ros__parameters:
3+
update_rate: 10 # Hz
4+
5+
joint_state_broadcaster:
6+
type: joint_state_broadcaster/JointStateBroadcaster
7+
8+
pid_controller_left_wheel_joint:
9+
type: pid_controller/PidController
10+
11+
pid_controller_right_wheel_joint:
12+
type: pid_controller/PidController
13+
14+
diffbot_base_controller:
15+
type: diff_drive_controller/DiffDriveController
16+
17+
18+
pid_controller_left_wheel_joint:
19+
ros__parameters:
20+
21+
dof_names:
22+
- left_wheel_joint
23+
24+
command_interface: velocity
25+
26+
reference_and_state_interfaces:
27+
- velocity
28+
29+
gains:
30+
# control the velocity, no d term
31+
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}
32+
33+
pid_controller_right_wheel_joint:
34+
ros__parameters:
35+
36+
dof_names:
37+
- right_wheel_joint
38+
39+
command_interface: velocity
40+
41+
reference_and_state_interfaces:
42+
- velocity
43+
44+
gains:
45+
# control the velocity, no d term
46+
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}
47+
48+
diffbot_base_controller:
49+
ros__parameters:
50+
51+
left_wheel_names: ["pid_controller_left_wheel_joint/left_wheel_joint"]
52+
right_wheel_names: ["pid_controller_right_wheel_joint/right_wheel_joint"]
53+
54+
wheel_separation: 0.10
55+
#wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
56+
wheel_radius: 0.015
57+
58+
# we have velocity feedback
59+
position_feedback: false
60+
61+
wheel_separation_multiplier: 1.0
62+
left_wheel_radius_multiplier: 1.0
63+
right_wheel_radius_multiplier: 1.0
64+
65+
publish_rate: 50.0
66+
odom_frame_id: odom
67+
base_frame_id: base_link
68+
pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
69+
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
70+
71+
open_loop: true
72+
enable_odom_tf: true
73+
74+
cmd_vel_timeout: 0.5
75+
# set publish_limited_velocity to true for visualization
76+
publish_limited_velocity: true
77+
#velocity_rolling_window_size: 10
78+
79+
# Velocity and acceleration limits
80+
# Whenever a min_* is unspecified, default to -max_*
81+
linear.x.max_velocity: 1.0
82+
linear.x.min_velocity: -1.0
83+
linear.x.max_acceleration: 1.0
84+
linear.x.max_jerk: .NAN
85+
linear.x.min_jerk: .NAN
86+
87+
angular.z.max_velocity: 1.0
88+
angular.z.min_velocity: -1.0
89+
angular.z.max_acceleration: 1.0
90+
angular.z.min_acceleration: -1.0
91+
angular.z.max_jerk: .NAN
92+
angular.z.min_jerk: .NAN
Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root>
3+
<tabbed_widget parent="main_window" name="Main Window">
4+
<Tab containers="1" tab_name="DiffBot">
5+
<Container>
6+
<DockSplitter orientation="-" sizes="0.300292;0.300292;0.399417" count="3">
7+
<DockArea name="...">
8+
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="LinesAndDots">
9+
<range top="0.717500" bottom="-0.017500" right="10.379761" left="2.746329"/>
10+
<limitY/>
11+
<curve name="/cmd_vel/twist/linear/x" color="#f1854c"/>
12+
<curve name="/diffbot_base_controller/cmd_vel_out/twist/linear/x" color="#d62728"/>
13+
</plot>
14+
</DockArea>
15+
<DockArea name="...">
16+
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
17+
<range top="1.025000" bottom="-0.025000" right="10.379761" left="2.746329"/>
18+
<limitY/>
19+
<curve name="/cmd_vel/twist/angular/z" color="#17becf"/>
20+
<curve name="/diffbot_base_controller/cmd_vel_out/twist/angular/z" color="#bcbd22"/>
21+
</plot>
22+
</DockArea>
23+
<DockArea name="...">
24+
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="LinesAndDots">
25+
<range top="52.434211" bottom="-0.855263" right="10.379761" left="2.746329"/>
26+
<limitY/>
27+
<curve name="/controller_manager/introspection_data/values/state_interface.left_wheel_joint/velocity" color="#1f77b4"/>
28+
<curve name="/controller_manager/introspection_data/values/command_interface.left_wheel_joint/velocity" color="#d62728"/>
29+
<curve name="/controller_manager/introspection_data/values/command_interface.pid_controller_left_wheel_joint/left_wheel_joint/velocity" color="#1ac938"/>
30+
<curve name="/controller_manager/introspection_data/values/state_interface.right_wheel_joint/velocity" color="#1ac9bc"/>
31+
<curve name="/controller_manager/introspection_data/values/command_interface.right_wheel_joint/velocity" color="#ff7f0e"/>
32+
<curve name="/controller_manager/introspection_data/values/command_interface.pid_controller_right_wheel_joint/right_wheel_joint/velocity" color="#f14cc1"/>
33+
</plot>
34+
</DockArea>
35+
</DockSplitter>
36+
</Container>
37+
</Tab>
38+
<currentTabIndex index="0"/>
39+
</tabbed_widget>
40+
<use_relative_time_offset enabled="1"/>
41+
<!-- - - - - - - - - - - - - - - -->
42+
<!-- - - - - - - - - - - - - - - -->
43+
<Plugins>
44+
<plugin ID="DataLoad CSV">
45+
<parameters delimiter="0" time_axis=""/>
46+
</plugin>
47+
<plugin ID="DataLoad MCAP"/>
48+
<plugin ID="DataLoad ROS2 bags">
49+
<use_header_stamp value="false"/>
50+
<discard_large_arrays value="true"/>
51+
<max_array_size value="100"/>
52+
<boolean_strings_to_number value="true"/>
53+
<remove_suffix_from_strings value="true"/>
54+
<selected_topics value=""/>
55+
</plugin>
56+
<plugin ID="DataLoad ULog"/>
57+
<plugin ID="ROS2 Topic Subscriber">
58+
<use_header_stamp value="false"/>
59+
<discard_large_arrays value="true"/>
60+
<max_array_size value="100"/>
61+
<boolean_strings_to_number value="true"/>
62+
<remove_suffix_from_strings value="true"/>
63+
<selected_topics value="/cmd_vel;/controller_manager/introspection_data/names;/controller_manager/introspection_data/values;/diffbot_base_controller/cmd_vel_out"/>
64+
</plugin>
65+
<plugin ID="UDP Server"/>
66+
<plugin ID="WebSocket Server"/>
67+
<plugin ID="ZMQ Subscriber"/>
68+
<plugin ID="Fast Fourier Transform"/>
69+
<plugin ID="Quaternion to RPY"/>
70+
<plugin ID="Reactive Script Editor">
71+
<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;"/>
72+
<scripts/>
73+
</plugin>
74+
<plugin ID="CSV Exporter"/>
75+
<plugin ID="ROS2 Topic Re-Publisher"/>
76+
</Plugins>
77+
<!-- - - - - - - - - - - - - - - -->
78+
<previouslyLoaded_Datafiles/>
79+
<previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
80+
<!-- - - - - - - - - - - - - - - -->
81+
<customMathEquations/>
82+
<snippets/>
83+
<!-- - - - - - - - - - - - - - - -->
84+
</root>
Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# Copyright 2025 ros2_control Development Team
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
16+
from launch import LaunchDescription
17+
from launch.substitutions import PathJoinSubstitution
18+
from launch_ros.substitutions import FindPackageShare
19+
from launch.actions import ExecuteProcess
20+
21+
22+
def generate_launch_description():
23+
24+
return LaunchDescription(
25+
[
26+
ExecuteProcess(
27+
cmd=[
28+
"python3",
29+
PathJoinSubstitution(
30+
[
31+
FindPackageShare("ros2_control_demo_example_16"),
32+
"launch",
33+
"demo_test_helper.py",
34+
]
35+
),
36+
],
37+
output="screen",
38+
)
39+
]
40+
)

0 commit comments

Comments
 (0)