From 9f354dc6bc1956bf09d97239a9f9a069b3721ce8 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 29 Jan 2025 21:19:18 -0800 Subject: [PATCH 01/10] Initial commit for chaining diff_drive_controller with pid_controller --- example_16/CMakeLists.txt | 84 +++++++ example_16/README.md | 6 + .../config/diffbot_chained_controllers.yaml | 106 +++++++++ example_16/bringup/launch/diffbot.launch.py | 159 +++++++++++++ .../description/launch/view_robot.launch.py | 111 +++++++++ .../ros2_control/diffbot.ros2_control.xacro | 34 +++ .../description/urdf/diffbot.urdf.xacro | 20 ++ example_16/doc/diffbot.png | Bin 0 -> 19716 bytes example_16/doc/userdoc.rst | 199 ++++++++++++++++ example_16/hardware/diffbot_system.cpp | 220 ++++++++++++++++++ .../diffbot_system.hpp | 66 ++++++ example_16/package.xml | 46 ++++ example_16/ros2_control_demo_example_16.xml | 9 + example_16/test/test_diffbot_launch.py | 104 +++++++++ example_16/test/test_urdf_xacro.py | 77 ++++++ example_16/test/test_view_robot_launch.py | 54 +++++ 16 files changed, 1295 insertions(+) create mode 100644 example_16/CMakeLists.txt create mode 100644 example_16/README.md create mode 100644 example_16/bringup/config/diffbot_chained_controllers.yaml create mode 100644 example_16/bringup/launch/diffbot.launch.py create mode 100644 example_16/description/launch/view_robot.launch.py create mode 100644 example_16/description/ros2_control/diffbot.ros2_control.xacro create mode 100644 example_16/description/urdf/diffbot.urdf.xacro create mode 100644 example_16/doc/diffbot.png create mode 100644 example_16/doc/userdoc.rst create mode 100644 example_16/hardware/diffbot_system.cpp create mode 100644 example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp create mode 100644 example_16/package.xml create mode 100644 example_16/ros2_control_demo_example_16.xml create mode 100644 example_16/test/test_diffbot_launch.py create mode 100644 example_16/test/test_urdf_xacro.py create mode 100644 example_16/test/test_view_robot_launch.py diff --git a/example_16/CMakeLists.txt b/example_16/CMakeLists.txt new file mode 100644 index 000000000..81ad5b39a --- /dev/null +++ b/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 +$ +$ +) +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() diff --git a/example_16/README.md b/example_16/README.md new file mode 100644 index 000000000..fe2445172 --- /dev/null +++ b/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). diff --git a/example_16/bringup/config/diffbot_chained_controllers.yaml b/example_16/bringup/config/diffbot_chained_controllers.yaml new file mode 100644 index 000000000..063c9f2db --- /dev/null +++ b/example_16/bringup/config/diffbot_chained_controllers.yaml @@ -0,0 +1,106 @@ +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: + - position + - velocity + + gains: + left_wheel_joint: {"p": 1.0, "i": 0.5, "d": 0.2, "i_clamp_min": -2.0, "i_clamp_max": 2.0, "antiwindup": true} + + +pid_controller_right_wheel_joint: + ros__parameters: + + dof_names: + - right_wheel_joint + + command_interface: velocity + + reference_and_state_interfaces: + - position + - velocity + + gains: + right_wheel_joint: {"p": 1.0, "i": 0.5, "d": 0.2, "i_clamp_min": -2.0, "i_clamp_max": 2.0, "antiwindup": true} + + +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 + + 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.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: 0.0 + linear.x.min_jerk: 0.0 + + 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: 0.0 + angular.z.min_jerk: 0.0 + +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 diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py new file mode 100644 index 000000000..d6f9cf9c5 --- /dev/null +++ b/example_16/bringup/launch/diffbot.launch.py @@ -0,0 +1,159 @@ +# Copyright 2020 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="false", + 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.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + + # 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], + condition=IfCondition(gui), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + pid_controller_left_wheel_joint_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["pid_controller_left_wheel_joint", "--param-file", robot_controllers], + ) + + pid_controller_right_wheel_joint_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["pid_controller_right_wheel_joint", "--param-file", robot_controllers], + ) + + robot_base_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "diffbot_base_controller", + # "forward_velocity_controller", + "--param-file", + robot_controllers, + "--controller-ros-args", + "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", + ], + ) + + # 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_controller_right_wheel_joint_spawner, + on_exit=[robot_base_controller_spawner], + ) + ) + + # Delay start of joint_state_broadcaster after `robot_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_controller_left_wheel_joint_spawner, + pid_controller_right_wheel_joint_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) diff --git a/example_16/description/launch/view_robot.launch.py b/example_16/description/launch/view_robot.launch.py new file mode 100644 index 000000000..41589297f --- /dev/null +++ b/example_16/description/launch/view_robot.launch.py @@ -0,0 +1,111 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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 +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="diffbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_16"), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + robot_state_publisher_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], + condition=IfCondition(gui), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_16/description/ros2_control/diffbot.ros2_control.xacro b/example_16/description/ros2_control/diffbot.ros2_control.xacro new file mode 100644 index 000000000..341df177e --- /dev/null +++ b/example_16/description/ros2_control/diffbot.ros2_control.xacro @@ -0,0 +1,34 @@ + + + + + + + + + ros2_control_demo_example_16/DiffBotSystemHardware + 0 + 3.0 + + + + + mock_components/GenericSystem + true + + + + + + + + + + + + + + + + + diff --git a/example_16/description/urdf/diffbot.urdf.xacro b/example_16/description/urdf/diffbot.urdf.xacro new file mode 100644 index 000000000..f11dc0844 --- /dev/null +++ b/example_16/description/urdf/diffbot.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + diff --git a/example_16/doc/diffbot.png b/example_16/doc/diffbot.png new file mode 100644 index 0000000000000000000000000000000000000000..1c4fc2476dba992c2984f32d8225fadf4cb9eba2 GIT binary patch literal 19716 zcmeIac|4SD^e}!?O0-N8k+sbzA*n23vW+ARO17x%Eh3R@BV@}o)lkSfh>#~s3t5t~ zRg9vL$P!`5n%(bQqv!d)zt8*E`|taC=kxS*+;iR6b@p?vbB@9c^@;1a1i3H_TX$4P z(+I;>w!)vSob2#~VMSUCf33N!V|E3@)>ooGY*BmGZ^bY%?5O4ulWPx$+AXeFcwU$3 zFT)#k9!=U&$Fn!UH&12fn`}wXJY#0f4(~?eEBiWZe%jaf`p{?C^Id+fNZlQk_5DBz z{jA4vrA*q8N0r;ROAMa9W`_y9M?-lP`Rjs?JbZyk*~BOGsjp(goi|I>S)48%^vxX_ z8d?eXJa`gQU;uxU<#>o}7^V}T0O%9ADoOCnNQ?rQQ`ud4;d|atIy_1x2yp3NSnS#UB$W#MEdHwi&$(arN3X!7mYG*6Nnml-Qtv>khvzIfr zzc*T{FsQrRSe23$^qh)RfV9&-Rg`%gUm!11wr#03#3GaKM(t9#%4>L1&A(>z1^n`MZ zeSXWhhuT1sF|q=^4%!ppo!qNd*ZFO@TA4gm@QJiJ(GX{pD>8xsqMl+>sQwj1}#+<=olfr(tM)P_`SD2rK6qe%d{*~XRnyyY!WX&T+P)MdPd@?gG2uP*rEGDM{O8sT z(<@`-{)*H23s+mKQi4SGNc;QbcgLMQ-)+|)+t*T}RG2v3;ppqXld@0ar@M4r5W_v_ zy>G;0I zKNZtu<&+Md=m?|zZw4qdSNkPR(T!1hZaehvIz+Nv=8A@6DL(ftLz_2a+H zRL74wbG^K-L=-qUdN7XY>i1b-9KUE6)g0)L*;b(AHrVQ*g>|gUr?hiSmx+hOf#p2GUuFxklLwG;u z(qIoDJ?B}oDr?((^}5I4*`FDSuewF` zKWl%#aInq)@$jE}LH-If&H5URBjbA~Omm)m9u#%Cq*`-#(*)us9U zOQ*KfI=-A$Tk2aX?$~5Pd};H(PHyx5=oPk_7Zz=cxVy2- zk+LR6R(O-Nn_SQt=Y-6r7>%d8AB^kXFT}bWI+QrQn&W8ZmoFNeiT;at=B5DAP`a6X z#gM4@rgsd`odxcAOJ2pA8{48{C1^1gb&2&ZCMj8Tdf7eue6P_j+4sw`r!KrODJf0p z6)&RQQ&3V_^tr`up-fWUNQ1^bvo;hwS ze9jTqiBASgc~|EeWg_nT9S~aDoagk4E#F(E%`t${|2TL^$Cl~f)Ru46sCUEdX+v;h zcyWJkV{ZO^zhL2|&8z%tBF3Yn7x&!qGE1kf*~C*;-O^sWUXN0k*m5Wg*l|!lcnvK! zV%E$#RNCx!T*rKRhlW?HveNXi(+_jqnPumPzRHREeVzXECZ0LBVE#Pr1HGAbEX7}U9 zYe$Kz*F2Cl#<2Ab&fEe3yE)F7*)C<++`sH^Id7@1@p4|S`B_| z3a?e?KkL^YGQT!vI``nJw~WZhkbA0!W%DSxyY>}3*ZXfdp7hjXKB4cJf3L?cnYP+b z2i66e&8@NH+v|QutA>6o_G*A$n3|F!msMd=dLYlfUko2^&5!y0^a6vG+Ts-TXO*JD zJ#rtZ0Z*KEo=tz#BcAjKc*pp9Ppd}8mDh-xxlN+ma2qAGGHmeql=0%qZ8PTm>o-MK!Wydk4obR(>F)6y8L94a`v@#_%nYz;!fQJ4nx^UA9`%8pU6;*v zP`ZYTM@qMC%A0`3KMThP4c+nHNO<_xN7pH{jMaXf9Mwz3(Q_f)iJtVx$6AR z3Y=9fLC9_!uVR4ICm-%jCn!&T)kKGO`2NvPGjh5ZAoydm7=I;6+twXN-M(wXk(Qn! zO(8pmHhtf%g08W`--1|CMI`e+0#;R>kUz-aY!c(;CJS=7xwEoO-&Xn5%<%Chk zQVy>43=MZ`G> zZL#KIJjLUcM)xjBfdTMrY@+{qOSuu&LLnxiz#9Df4T(l`B#&Gac*8Mgv4| z@^%fA%NlClTG0c^jd@d>@Q=(d=9pXE(Td5CqJ_>$cH-%`s@nTwAU`sS$u-%{h-XCF z=L;;fTo5VzzP9f`z%+5K+-~Z{SQoa6nWX4?HGkSv7{Qrq3sHjE_@a%}bfLHm{4DAB z(6v9qf0#}g1H+vBT|d)e1ghokgXWUyyA)|f-f zjwwM<3zKmVnCBtR?e6iTEk2cve-T&x@UhsidTaK=%FxUVZYvx4GzwjN_0CJvFXGCB zi(m6?d!Y2;IksI^lV6EfjbdJHwj;CB$|AJ9N1Qw=kiy*_jw5onoH({yPBnUFen#?w zc*+IXOQ+e3?|>%1G$g(!dQ!2<_Sf|iTFrj!dX!-427{Yj8QUaoWtyCenJbBq;dTW9 zJy@^de&(OdcO-hCSXth|XvMGU@@s*l^m?4t3zIQd6Vtu2H?rThzj6@=zKU7p;Vbv) z3N@YJ?$Li*C1<)j3a6eX*Abq?ESo&PbhO&^99i<829C3WyhLSbX|C{xn6I9f{)vgE z>4iUPoZ=?C#~A^Fk?$h->meD0*j-;n&UJs4yH^vtXP?YniPWghYIqa|kG>cQ)Cuhp@(f|xip)zJ+QU~0 z@Kul5+)*w84=ku!=w~-j&1kQ(W z(5(M1|I00PLg{(I_>oGIO&3_U*xYh_2%6~KHq3WUDqn$XM}4aSHkNkU+C_fKmXyBz zi|e75kG8J7TkgZgJzvtet;9U@o&`H%@F@1D1r{zo87xgz-X3W;d`yT#`pg&(3$_iF zP6b9O0S5>_fRAQSGyqjMl)l-ot{qA@R039cCh4N8>GO+ zVat@j+`+))bcBMxb+MC_$KkZo&OQYxYj zSi3nG3SC^dE;%bXKkrf4?L>;qV6G8;UZ$v!9iC@P1wK-@Gt~J5BgAf>JuvueX}*2X zVf4t-w3b;OVt(~@4R`U8U4BeQBho?lv=-Nt3oo))#IPeGIDlbnw%2JSQ^kVq61PO` zvYMhI>?}ynszI9M8~Zf*!^JN;eJAN18E9r7Lj6d^;tL zTg9Ru+-L{9II)bArp|>2xUG_GDj8kX6{-oEDcL|8L0Q8+Iir%KuA^Lga(7g}Z`=p_ z!lK3tBEh)W5B61_!bf;9-D;c_Ue=Z7x^AnTa(H5t?oO^W$e+y$%`es`b`Nz+HWub# zhUS2BoI-8o+r+~YU2n%DMwDh%6QjICbt1e{cxYK$r<|J+1o3;C-=ZS3OlstH@CDhB)?6=GDA%5#k zi@nO%Y0N8UFgVrp%0+^$q(O!<(+xalt(AFs7kED-4B)~D7(u0g57o3 zRj7)P__n-<#eoV+5K9BHH`)bAhTiP2O@T%3$Q5u&ag(0SHKa({>J}CmW#yRlv%7C1 z-gAH$DRfk;h1!o8de<&gT9&M!Bn?`X?&6kZR~44`BX7Meek-8)x{{IXRLW6ZSfkY< zdpYze{#Z+ON_ko(D=1&$MrLugHK3Jx$uv<#~8~JWN*yBC4na?$3O%YIZrI5+QRd4 zq~Mk#8=hT7f*?!IjIpGxTM@FQPrg=L!`+T#V;d7e2VP=cCXbu)Sl3|)i?Znxtm6nZ zy?bf}Q@D57wYr*s%@A%NVLT9jN1{c}emVW>nuyf_rOp>Ox2(7ydrb&gk68Xq!LycG zIZe)IAXZJhPn{YK%`-HwsX{Ek!XJ4j1H;N6yo+QV%6qzt+e*_+>=q6&0i)?Q^A+vb zY^Pz$GUPfXYQUxYl?ZQ4H|rX=)vo8}&&;h}o_e;~?uQ%HCmtm(+WiAwG>InV{!8{H ze(S2??u~Ym{~U33KiB=^^T!n!c_^c)*}1TA>b2Ib?bca#XPnbZJb5wltwE8LW8vwq z%%{4Z4t1Z&Lp5MO+BD9p@j$%zyY}1Rd6nW&5dIywtlT^`NYkEuMa$F`tWjJIt5&Bm zRSWQO2cU*aXc0&`mL8hdi(aC|O6lJD|50%aA_#_;0iR+Ay7kw!O|3>R9tC|o%96>S z(H@oBaHt2`*(P(_Rw}TR5b=6bcph5$OB2bfx)^EErNW_pk_Ut;EJ>4<8-Gye7t?bM zbyY(v%|JnR*ny|phOhw*Bs9NI^@Vbz-a{!aTj~HTd99`4e(0b`y6-lO9~HZ`PUhx@ z;MOuYATj}}*^bE(ueAsVArHYgAnH+lwFTHP-bg8~o*U@(WH_NG7(uQqti2K-415}LOrLD-{lxX6?Cy&IMVGng6`Xr#2|cQ zv#>;W=CPzzYJ zp$+1OHUw($w_F%iYjU;-sCtU1YT;SOX#eK3ErR{6J#8aKxLKV-@IvU>1_Pp*Ea(P6 zEeJ}j2M-g3NHhZDFw{o}WM=-26J$3dVGc}($f8R37@~#aW&U@DpD3#xb# zw!qp^mvv14Q8>nC+sFJ2_@V?Y*#Rh$kFlUkbF0jzW+`{J7c|8FvnpXmU48=CORC565{}(2ra;{ zOoW*3-v9WG7Y1DLG8*n|Mmu2dohMlQrfx$|;~@e1`E!#Qub39n?_SUb+aTE8a2trn z7M)|r$4a)uwgV1iBZc}D^aWCULm$2(jAcuPw*Puf!YYJ)@aA=lC@r=;BqIDSI9E;| zLEyy2=0Lxb96>VG9H8HXWH1pbKSJCULE908%!aCrpkeR~R3d^(L4&Er2%;d!8bPNK zbRP7ZdJItNi3g?XF(@g$ba^A;JBWdh0Covwyy_Ip%K;cp-I|9d+(8US21Iy`AY?#< zQ3RPe03?o(j{$KINC*=6H?72Yqkvwj7K;%{<%kjQAsRQZJ)(q0)pHbnq`$_{P%G zl(?|ePL@Y8JPjn57XU*DAYGf3Xnkv$_)J=6GW z5LZ#mFY4Ue7?^d8`KLN77~BNL1dMo^MKtGBlDludGG7JEYk-T6*ildYgfj1WpZEL% zin*)q9LTaIVMV~m9o>y7j(JrBFGeqYxsqbJJJ&t{>|g>^N%o1;D#momcvzxoV032< zXe!wcHh*_l$Me!|r7`FJPdrk&6#Cx_2X-CIU<>JBM9P}Grd4N>xv~4`f%%U>N4+s; zHBcs82w;DCtSw&IxA%P(;Dx)Vz(ue;_9)lCd0vT`kEcEGI?&_5kp(taDM4Ystl0k2 zspbpUNl>vZ;qdNGzvg*!+=uL;Y#X?JX>jJ@u~DF4qF0V_c8dqcAg9*>)=dP{*hpmS zKjL{Qb8Va{-8)5j9fQNh7F3C!GsS&S`0h4Ts41hZ+}_oI)6|o zF`>!%eBFru1z_ZM5HjZx676e>l%Tq!{M6!(2zJ?3uDnS@D@k4F6CReR%>D47GaOBR z8mkCnV{kXcAAoJXBxteO_;le3NB72;E}D%G7S!MHx#rCmD-nNm!tr2DVH!rGDsUMp z6!Zagy4J6;oDDeSL3Co;OU^dWGCP~mln>>R+^8)rWTfVHN@9dgUHk2 zwv1=jBE8#Z&ZkmTBp-HZ|xzuEiZtX3lkB}Ez9Zi5H0uv6Bm)4*(5{!&%_ITJh=U|Li>vJSPKe8?IOpOjrL%pIWnS)BO}(+H|DQ)4-LM%eWt5p>m#O1g$=Ot zO&nMO3qc7#NJ+e*LRX868voypwZqXM`C2>&?G-pxXb#BALlQmrn zL;*E{)YUIULC$ceT+M)Ej;9IdfLR#s=<3Hy8FEx-v)?}f#)fW|L1sx@w|+7fyJp9Z zv^tP{6xSh>H`M9sr=hKVTZY&Uf}*DI5RL7rJOt^~6DO}e#bX@1DE9XOU$1@1laI|l zrV#UwIVG}VFC{2HZ*x4xX5?D0H%U2=Fr6R;bD>#3Hn&ICW|O1eTb3qst1{#HFuh;} zb1ry}k+U}vk9}Z2QB5J{?!C^2)oKZ)9pW~DFT!|;0u!Y5Q?C}o6tYU0sUee zQIZ|)D+Xl;u8-^|Fsyo)EA4I`VKasdsDfjaGqc$TChYF z*a&%UD4puUm%)xvjl1x?_t4h14;8r5&LOf;`9|#@Km_@NfMW<>8y=`2xzGCDo9r)y z>K-91luj4}(P5oNU3l3OXeU&8(u23$dVUYMwLXH74QtY)oT-iUh2YDFnD=1NgbylV zdje?~<_3N*{2fqDw!{$w^6)2s0#v8LkPOhw$pjFImG?j@NdJcr)!3T(tv%=4(K#s$ z52GkzN4upA+e@aUsYk%%)_~JW7q{<_v29@t-32|p0e*mH)j#Je(PSL32#S%gcX1mt zA4@W{Z&U@24}wEORXl7JMCk`O*iS=*DUKto;n1pq+Ew%$aA9XvqZ&W_AR3+ z4F(s;6>yZ;6_Js6SPb89f^dVyd+hdvfUpZF!lb|c51vrC`lS~MnaDf= z_K$`A#a4nfWP%6B;BPclW9R|fWr7=&>tQUaw>~2Lf(!*YRr1(fEjFDy@bb>9^2J~@ ztPHKT-_he&OP5TeDmSJJ2a_9tl4>-hQ>z6obUSi{UIk?jrX&r0l%tA~4g~>I37Log zqXkpvJadq_3B%IQjJ{*$QnCPn7PzJnBrO+479Sa0`nFzNUN91;PJhbZ-r*>3{+x}4 zI=(c%wmr9O`Z;cKZxAG(LAb|F&J%_?&ras}3AJCkoi3cY8RG%Q5MDD)hKElM7kt0P zV{h@%r}27*xs}uHOz|Lm<};%4zRl!RAiQPF(cjf~MvR%Cq@a}7Zxi|Md-c8ppmH`k z5P5l<4b9xiY|9T!&;w&5c(8BIFYv~0!b{K>LxOR2O5j_`3>e~Q8VG;DWoV9;9Up*Y z5gaGL`J_Skv#q0liMV}Q=824O5bnMF*_GfTR=^>6aS)QlVq%V`I;Z%nPel@3X@hkk z;w$$>y>^S$S`DV11f?xEFLE{pp{2T8Z}w?GN-SZ-+Ofb&R3L{I}M6UAAM@>?e@! zvyD0X`%1X5R0 zxd89nXV0F!^S^^nN~rqGo%UHQ{b!|1VuGLlghEltt5@H~4Ych8cC$=qLRKt&VEO~C zLseyAvA>G@IGt~JSbLb_SoG?ZR63;wP(c~dOPY?8`$|`7y#?#<)=$Ijl|Ufa*@4f% zs8ERiVr)4Nii(P6tt*_}9p>JB##PKV20VyOng&)s*mo5^?^kSGYsB6URwFCPe<=NC zbK~c!2Sugs4Mic-eq~n<`tb=TZBHK_8S(%9di6bJ%NlU@v}j1t3aoBrs@Z-vzsL-1 zi^#IrKtJ^MGaI}y_4ebh-uh!77Z(?so1+DPO~4E2&PqMD07W`gJOlhBUxiVhUYrt&WpU!v9_Z_9~ySxf=W~dBBr*29AjZ=FwiqPVW~ml)$}}x^UUas!zabGY49pdS}}6IW#z~}?2y#| zmHqD=M81OBVXIr4pB?$3r{AD_#8-Q%m5 zp7*PbkB|R}joWXq_$%bx!mp*cnNMnw9xq0h?Q>{EO@DA@>q{RR3hhEbXJzI4l%|2& zPAMg2W$)f^H8nNASf94%F7+Qj+S}WsGY=i&SJ}^%_QC#2#`s|cg;Qo`Ub7RwK0PvU zX)n6EFguyJIPplh-fQljnm_!xeMxlb_L9TkVwV5xqu*6aQ*W3bIzFh*v>S$qG=ouG z>ea9IS9vH0@nZ`p1mUc7B)!k9UIFVxDIdnK{|nK{ydrpXtumfi9&pyMl1Rjav_0D$q}A zYfGG;8TDTBt%}c&D{E_MiEp2O#AL<~)OCd5oF(edCcMYMr0e!VaZ53Q<64Qd{ zrh?3);MJ>##cpO$j3DpJxqQ^Ye|3jTIFY@87?VJLE#4P&_<6 zH8?{;LL?+4s;a8QgSxxBKYjXCQc@BV6Qdw;#Rmj*`JWdrrU#m`T*?*>7SZIn(in+< zhTDt1#_b1Zn9u3kwfk`DUt3zjE)9v4mR%{>6#ac^RMvmqB~rVa9=wpBpFf%A7;W@z zUs;ZnAWd6R+PBQm4HQ1i%*u-C<!`dS^7`N(z-a4y& zg(?v@rQR}z9?FPdy_Z=vB``IBv?R(7q4f1*+9FHOO1cS7@{O4S~v7oHxA+D7D z`}hAD=_vCYIX%HOQnuvB`J=b)KA3P$c}s1I=@ZMabb${aKPEhUXjzOnb{=dBJ$*9u zt+n;~)R5VPGh*lDKvSpTbswMkg+CoK($d=XE;7{h$|91FKRuiOW#F%*sOTihAyoDD z!R!iG-Y(GgF{mqw3ADSq6Zj&FF6Ua8iEP%=;LOU(0vaPl7Jk3k;PU;|%jf9h36gos zU^1DwiX4mQDShwj(TAvZw$x{4Y;L4%LtlGEj;3JLQ4P+(y42OIkG0=SFX$vu)c12y z8GV&{M8 zGB*=7IM1Iy??3%UR7qb_H+7`s&uRHV{Leka z$L!crAuC3Ge85A#eCu32DTM#747EP^vn^cqk80ApCu3h`;I_Z?b1zrFU+q0o!r$(=j&ae+O0HE-AwpY;D>Wh_LO9I*;>C-0 zcDs+fb`p2iZ%Jtz`|dqb*wrtQ=)dkc;OaU$#cb>FUs?c%`0d*__&;dvrS_u2k`m$LhXoby@W^>F zj_%k8-Ua% zEe#8yb>d*Ydt0a7+}%H4IB3CrxZ8a|I|XgavrgzSjHA@~O|>}W^nEM3I{0RT+O210 z-^%>`&!69;#Pz1O+J%r5G_ha=nJJ+(=gAXQzj=im`cjkiLeH8}ICjZ4R zR$>=F$a9H99GKL5hS}K!>=uAa1FxpLuv5VmJ4j`ldRqLHHZ zY7ZlFxY1Ez-xGwyCR+_GfRaEChr*+TQ3Zl z`p-T0->Uus91u`$xSAa=5lBN9yd`kPkK#s?^G@t-hh_|YgOhbs(`%R5k z-Co-3XC+_2Q5PH>jKW~wd%rXWW8#&Me2`&Qlew$B`HoHMFRt``9Pg{o@5qfSw1@eG z(0cY0U6A+pC(~)>xw(e~z2o8xwAb$d&ijEK(R5bxtA)g$bI)`xEGj8!zi`k%Uq8kB zyYHWN%Sr|mCjv%)&80yxtz3*%Ugxu8Z2Yd`++5;2=P{UY5`WwylvH0wgggyC!{1BV zzUKl%uJq~4cj=T#6Fj!0ny5pD;0wK{$=ND&bY04iS7v|OJv*2$mP-D7_nw<#M(r_8 z;j!~hcKfQREu3;~2DI(uM@us^fj$mnTA{OT^T*ZrHQXlLdO{pY&!ISW(qe9Yynff8 zy@wy4e)j!gOw5{#uJV5Ea=Q88I@>xrZjYbNxpsK}{&RDVP~xN>aOHYD>z9w-4q;7) z3G9z}_Jp1JgfiuR{kmZ5?zHwV9tjBv>tA$U0XG6Rp(0x3HU9JCNBub!hWz&UA{aTL z8l8r`*j=eU<@P(DcwoQgq1IgC$h8L&p6`79LxUkthe*lzm>NXW4q%tnkvtFAZs`nk z5$JYc;MDV54zqHc&?RYzN;B%ay^>h>EYow`zdjkpCHB?R@wnIA~sI(F!3T4VG)sCr%oJu zZ1(5(@88qY3f*apY{<%v&+%e%njJ1sLc(@ML8QNO<;v1*?$Ydd;?g{5V~+A8|Cjc2 z4A>Xqm)hJ={=P%Cv~XybC2~AQcQm_0zk;P@PQUv49eql#4xZ%YUhO_WF?(uhX$g6F z|4hS&xI>AmzD4=@SFT-C_$Z!!!OP3b-(L+1AYyEM86Hq|(A|3{Sb7C`Wcc_jE&Ku9 zyA<5`%lfI8yZZzyiBEhs?6o-WB^`U!)YMc|&F|rq88k%nPCD^tgwwI!sL$&6lP|cq z_)j*OGCZ57L|Q9_;UglV?SB*3n(xT@<81{0&n<6l)1?XKTxvQ;!FV#E5yG3-520Yh ze`&64aj@)Cdp@K!5DuVQ@d_y6Jmcr*mk}kaV)fFkx4$2+PaNK%J*QtWHs&_4Py~Le znb)3r7Zt%-svkY1e&1CySw0W$q_dNQ6wB7{C%k=pgxVF9l(@tT&N{evgX+jwmkz0F zRy8u}Rp+E)j1$8@cW3BZk%el$u%7_OV7cp%=JQ0z$SI*Oz)6nOkX)GLR{K4a^=aEF zH|5jkC{pcRsZv~qYg>lqHUsNy8#UbLV&dW=(gg~KTfmXFNPD}x%j<8w_&)O`o$M(pjqOC7m@GoOZM9#WIz!?fU&uY}L6NxgNtvFZR|H^|8}mHKe_Plm`g_a%C$>;5X|Q zYpn>^_PBhxVSEwNB!~J074NAbh*QkA=iNfvxKmt_LlV4On~(X(TAMlJAOxO<)qOY#^9w=a1HEvS0M)!c z*qk%?CAZ^+E4_PLBb`p4`XEgQR-#XPI|Qe;Z{KdoT@J{1R3j0D4Rx6@Br7?ta(6_B z{6etwKH+~aA`1&0OAFTXZ>`bg7Ya)>58|if!@jfbE-oW+E~QRoO72(SI)6_>JP58{ zqXwn2s|x0(d^lOvO_jPDHKuGX8# zR?!9EPwRe%kpAR^qQtV7F9p;Ul$Gykv55t;N|ATn6SPF_4n}BGZr`R4>Xc1)MNmRw zKm6qMK2l5^)4o{r=+UDUJW?1c`Xwpk{nN0sG%zp_t|yC~ok_}f);}oq?yZeV_~Ti{ zP+RzA0A~-RU{tf%t8C$7fg0}TloS-EX9sgZD6>C8M4t`c;z|XN9I>R7qu&meF+4n6 z>bGD{0-prqd(|cH*E1qVM!O+BM_Kz!XW#~PPEp?pG{`MV^z`(!hMPu++WZ+8mt3RF z_aA46S7C~sTMrUSwanz}0wcQM;jme?d6q6lMf*e@rNh_ z_k-KteHQ0uKmn}F{8W^cEiz4_qoa)!uV6MSfsqtDpX546d$>(HI6&wtU6_JPdJ9BY zNTZ)j_5%#ry8GPp^mKoJ|Ip9{C#R!YyqH_nAs6zi+C}gP1zmO~Gq0{sI#1Zsv55GxGB4_HFAuO_=o*v$0UB~-sQv1hEn`K1 z+#~&+IzLACfZo1dAardpVQ5+J`iK@@f)np8H|jAZCdeUZOE|1#XF<`h&kGh508l>* ziUBB;1;J=RIUen}1)bNxf)W9`j3DUUaLKNO4jpzEDvXuO0}B`xr%!y#BYj4gDAh!<_$y5W=RMvTk@Ia%I*SR%H$3vcMWJluh5`2lxf zUIP=e0yd-3C5$j&LJVkxHY+B^4Uij*e$=MGzzoce==QMs`509dvP~((ifUefrV-iI zVgPlrpj`k}r${+Ni;u7hW`xfmN~m`5iy&uNM|H!t>?~*wpcEJw!q5a#02+(L#E1ah zU_sUZZ9`Ovc>?r}h5T)Rd|1$pb{1k?O4LzU|Pn*-xb7{F5%mn#xZiJTa=31$mg&Z0F(p|Nxm2K`s@vtg);WD5gHFdlY} zwvnpJZ3Q!NU>iCyV6H41O_dZ(*#jHWpvpjv9Y<4t7(oDz`Gt6q)B|)oA-c&)N?dxZ zw>06=uduufXd`1QU3nh}qD>tinejhZg`j@4@F6RD#43T5x2f*ch$bNB&FoX@`5Qo5c$kWhfv;0q)u~Bdt zx1ghg*~mYpFI$U^EkG=5v4PR0ay(KN8$Ey;Sdb4u(2;>(3xUx$414!TiL3IjJ?p>- z4=w_+Wd}@Sy?~iFjL-$79{XQh1}|Kk*CHlko503=o0gd@E8c}~V0j9?SdunD0&(F1 zT#P!|ku^S84-kAJYw;5#3b2CK5gu zA0cSD5+m%cB;h5|wmSFit?=7fQ2YR%2EQx;ok?&NG&F>Tp(!L98ok17!KKhxa<`2f z!1E>|ve4KO??V;@?t_;lOIxTW2P4hGpv0xV?=L}A2-&>H|D7ts2+_djV3=~k(Abe7 z{JH{kT4C(S5XXXK09wg{U;v5g4bc1&v%Su>TL-t&FlR;53hD~ZSr_&dez-rbXEvyO zc68XuwNfx+@s$Hc&lOl>&t|xti8r~hMY?->IEfp>4m48Ka~fF=hLBc3H{7`$&ikyd z%ryyJJj{{o!MppOwtHArhaGfv6VSg4&^N=kfWxq_T0+6GTY!s60$q5MbFoiuMu1|% zF9vMB4600?dLVbBQ4v^6xCgt+qg@MhmAQI#x5G3zh8>4vlSRxvBW7F#I7|qHm*mw- z!bfZb&MPGg2j5_AF4~2sZL?$8y{rTGG*MnvWVmuQ()&mc-Y(YmAq8~Y`y-%Uv#G+t zI_u$_hP_ODB@2m?sw(=D0|%W4Vc?+7~7}Nv z6|-5v#1GxocK^%Zj9Us^Z7ifeRFb|rE;HB$@H_nz32w-tmh*Hsf^30V{49Jd3+Vrx z6hxD4;MSS=?6K+;M3mSDCVrm9qiPUrJ!@&8Bk_j?i(-W?ywBc$IedAg=wJmu5(b&> zMEv1`_*uTDfc{&Q1t|g)f*5*Pkcod)f&_p)`b4>b9^nYex+Axc#5tLGE66J`v2YKX zU)siYuTm{F__rOd@1QhFB1OO*6G|g~az6mZhCQt$%>vE$d!<3+2~x(|?wW`@ZjQuX z!H7F}*h4yXBNQtztTtIV&G)vHGq@~_D@8cX72!mPDWJ=r|4*N3N0oKsmh~BX+em2? zLkAkFGVwV`pOd&+NwNRK;!YUvWy(U*gl*nPbgGoMS_fX7pjAnF+=m$Zxtg}|>J=oo z-D$#UXA!rOPPdYrqqJeH5yLWQ9<<#mc^L@Jj3^J` z1u~r^+sHiaX${O2Xg z52`lz+Ocq*b0T_0BIP9TwUSPEB6dn3R#Iv7e^!}vybEv5lB&>gp^INy6e+V7#h za_z{n8Y|rMDDeEN#tKm$1?O3)k_zbYf;a0FN|h0}G#QjBd?YI;BZx5_Jy|T=*+7{> z0T|;-7G~-QGUEFzCNw}Im5fVB=veqRipQD1EKUifN3vLqzb8qLX0e#}m~yKWVlknn zfPV3>BQ}&`ItKn_v7t26@jQz)kCTKih9DLvZEIy{vo;^tMtLUoubUw3^~ttSVJU~8 z6LFB`kPIc6j&E6S55P!O6pQOk1@uW*WIzx|3glRHUTmN=S0c9$_ThK{aX4vbD?=&u zKQ|$^!L@*$rBqX)^t8W9oTSp___8r{IPV*>yd;(MTU;<&2(jZ6CAylm+hL}o>t8J{ zIIe6S{a1_S^_9(WEJXbri4n5@x#Ip15Bbx}u9E!3kr>EQBM2dpDBr`{8!0#cCpr)? zSNWSPZd~bdBs&!eO2ZN*2}(jO(iw6kjp4TvmLX8(`B3@#UK zWt?Nl;`dHw|FT^iP8L=PV!hN?$(T3dvx+W;WC;5$EIMe<;%m8JE8`(+|5HbmV_Dja z4LT9WQG~-ffp^PR@s*%l6oHtIwK)=>vvlsC7!UXD$S7?#F#GwOG(8hs7#qNVGNKe_ z<&Yv^b7Az{m}NIXUZ`Xg{>NeRf}TJQN-8H zsa#faow&HH#+ZKdRiUehP%+&yV{MiKtsE#2W;rNaR+WeRvW;K?Mdyj4%jpCmwX~Il zB#v=0jw!Q(MH1esfX+Hy_miK{aWi+bVdP#2ih@h0zOZB{SNM&hgbdFx+rDl2ZQW*j zJ?I9UmQ;-}Ez!yPH2ut~YN?yY4m z#F|D7F!8!rz@8)tIx0UWje9a zD<;c3lO@(W5s}ej33VNmbmS)Nmh9li*gJ|8lx<-)kq=$n|LV)B?FK@}|4rGjQ`_Ir z?jUO_&zgF+O!`+FQ{~v@_>G+|EcuV5R~OFxMbP`zxBrF1Hn+e0Cy{?XasMUK&8&@r z?#q~M!DC@KZ`?cH&J`-ZX%>~fF; zGlPZOk!BGX?u6fWf;v2gu|s7Mu6j@z!7xs!m7v55rui^27^MVVM88jjkufM{Vi?>1 rZ7)psK_QOe0;DA=xBj0pW0B*>ch2ISyfts(ee9@~zUC8kn;ZWh)^F|4 literal 0 HcmV?d00001 diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst new file mode 100644 index 000000000..37027a92b --- /dev/null +++ b/example_16/doc/userdoc.rst @@ -0,0 +1,199 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_16/doc/userdoc.rst + +.. _ros2_control_demos_example_16_userdoc: + +******************************** +DiffBot with Chained Controllers +******************************** + +*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. + +*example_16* is based of *example_2* which the hardware interface plugin is implemented having only one interface. This example demonstrates using a chaininged diff_drive_controller and pid_controller for each wheel to control the robot. The controllers are chained together to control the robot's velocity. + +* The communication is done using proprietary API to communicate with the robot control box. +* Data for all joints is exchanged at once. + +The *DiffBot* URDF files can be found in ``description/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst + + + +Tutorial steps +-------------------------- + +1. To check that *DiffBot* description is working properly use following launch commands + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_16 view_robot.launch.py + + .. warning:: + Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. + This happens because ``joint_state_publisher_gui`` node need some time to start. + + .. image:: diffbot.png + :width: 400 + :alt: Differential Mobile Robot + +2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_16 diffbot.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz*. + In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. + This excessive printing is only added for demonstration. In general, printing to the terminal should be avoided as much as possible in a hardware interface implementation. + + If you can see an orange box in *RViz* everything has started properly. + Still, to be sure, let's introspect the control system before moving *DiffBot*. + +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + You should get + + .. code-block:: shell + + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] + state interfaces + left_wheel_joint/position + left_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity + + The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. + + Furthermore, we can see that the command interface is of type ``velocity``, which is typical for a differential drive robot. + +4. Check if controllers are running + + .. code-block:: shell + + ros2 control list_controllers + + You should get + + .. code-block:: shell + + diffbot_base_controller[diff_drive_controller/DiffDriveController] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + +5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: + + .. code-block:: shell + + ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " + twist: + linear: + x: 0.7 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 1.0" + + You should now see an orange box circling in *RViz*. + Also, you should see changing states in the terminal where launch file is started. + + .. code-block:: shell + + [ros2_control_node-1] [INFO] [1721762311.808415917] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 43.33 for 'left_wheel_joint'! + [ros2_control_node-1] command 50.00 for 'right_wheel_joint'! + +6. Let's introspect the ros2_control hardware component. Calling + + .. code-block:: shell + + ros2 control list_hardware_components + + should give you + + .. code-block:: shell + + Hardware Component 1 + name: DiffBot + type: system + plugin name: ros2_control_demo_example_16/DiffBotSystemHardware + state: id=3 label=active + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] + + This shows that the custom hardware interface plugin is loaded and running. If you work on a real + robot and don't have a simulator running, it is often faster to use the ``mock_components/GenericSystem`` + hardware component instead of writing a custom one. Stop the launch file and start it again with + an additional parameter + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_16 diffbot.launch.py use_mock_hardware:=True + + Calling + + .. code-block:: shell + + ros2 control list_hardware_components + + now should give you + + .. code-block:: shell + + Hardware Component 1 + name: DiffBot + type: system + plugin name: mock_components/GenericSystem + state: id=3 label=active + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] + + You see that a different plugin was loaded. Having a look into the `diffbot.ros2_control.xacro `__, one can find the + instructions to load this plugin together with the parameter ``calculate_dynamics``. + + .. code-block:: xml + + + mock_components/GenericSystem + true + + + This enables the integration of the velocity commands to the position state interface, which can be + checked by means of ``ros2 topic echo /joint_states``: The position values are increasing over time if the robot is moving. + You now can test the setup with the commands from above, it should work identically as the custom hardware component plugin. + + More information on mock_components can be found in the :ref:`ros2_control documentation `. + +Files used for this demos +-------------------------- + +* Launch file: `diffbot.launch.py `__ +* Controllers yaml: `diffbot_controllers.yaml `__ +* URDF file: `diffbot.urdf.xacro `__ + + * Description: `diffbot_description.urdf.xacro `__ + * ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ + +* RViz configuration: `diffbot.rviz `__ + +* Hardware interface plugin: `diffbot_system.cpp `__ + + +Controllers from this demo +-------------------------- + +* ``Joint State Broadcaster`` (`ros2_controllers repository `__): :ref:`doc ` +* ``Diff Drive Controller`` (`ros2_controllers repository `__): :ref:`doc ` +* ``pid_controller`` (`ros2_controllers repository `__): :ref:`doc ` + +References +-------------------------- +https://github.com/ros-controls/roscon_advanced_workshop/blob/9-chaining-controllers/solution/controlko_bringup/config/rrbot_chained_controllers.yaml diff --git a/example_16/hardware/diffbot_system.cpp b/example_16/hardware/diffbot_system.cpp new file mode 100644 index 000000000..8cae96625 --- /dev/null +++ b/example_16/hardware/diffbot_system.cpp @@ -0,0 +1,220 @@ +// Copyright 2021 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. + +#include "ros2_control_demo_example_16/diffbot_system.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_16 +{ +hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = + hardware_interface::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = + hardware_interface::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // DiffBotSystem has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' have '%s' as second state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[1].name.c_str(), + hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); + + for (auto i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // command and state should be equal when starting + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, get_state(name)); + } + + RCLCPP_INFO(get_logger(), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); + + for (auto i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type DiffBotSystemHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Reading states:"; + ss << std::fixed << std::setprecision(2); + for (const auto & [name, descr] : joint_state_interfaces_) + { + if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) + { + // Simulate DiffBot wheels's movement as a first-order system + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates + auto velo = get_command(descr.get_prefix_name() + "/" + hardware_interface::HW_IF_VELOCITY); + set_state(name, get_state(name) + period.seconds() * velo); + + ss << std::endl + << "\t position " << get_state(name) << " and velocity " << velo << " for '" << name + << "'!"; + } + } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ros2_control_demo_example_16 ::DiffBotSystemHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Writing commands:"; + for (const auto & [name, descr] : joint_command_interfaces_) + { + // Simulate sending commands to the hardware + set_state(name, get_command(name)); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << "command " << get_command(name) << " for '" << name << "'!"; + } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_16 + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_16::DiffBotSystemHardware, hardware_interface::SystemInterface) diff --git a/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp b/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp new file mode 100644 index 000000000..0eeb99f70 --- /dev/null +++ b/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp @@ -0,0 +1,66 @@ +// Copyright 2021 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. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_16__DIFFBOT_SYSTEM_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_16__DIFFBOT_SYSTEM_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace ros2_control_demo_example_16 +{ +class DiffBotSystemHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the DiffBot simulation + double hw_start_sec_; + double hw_stop_sec_; +}; + +} // namespace ros2_control_demo_example_16 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_16__DIFFBOT_SYSTEM_HPP_ diff --git a/example_16/package.xml b/example_16/package.xml new file mode 100644 index 000000000..f802f4237 --- /dev/null +++ b/example_16/package.xml @@ -0,0 +1,46 @@ + + + + ros2_control_demo_example_16 + 0.0.0 + Demo package of `ros2_control` hardware for DiffBot. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Dr.-Ing. Denis Štogl + + Apache-2.0 + + ament_cmake + + backward_ros + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + diff_drive_controller + pid_controller + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_pytest + controller_manager + launch_testing_ros + liburdfdom-tools + xacro + + + ament_cmake + + diff --git a/example_16/ros2_control_demo_example_16.xml b/example_16/ros2_control_demo_example_16.xml new file mode 100644 index 000000000..4508456d1 --- /dev/null +++ b/example_16/ros2_control_demo_example_16.xml @@ -0,0 +1,9 @@ + + + + The ros2_control DiffBot example using a system hardware interface-type. It uses velocity command and position state interface. The example is the starting point to implement a hardware interface for differential-drive mobile robots. + + + diff --git a/example_16/test/test_diffbot_launch.py b/example_16/test/test_diffbot_launch.py new file mode 100644 index 000000000..6ab19d54b --- /dev/null +++ b/example_16/test/test_diffbot_launch.py @@ -0,0 +1,104 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import os +import pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_16"), + "launch/diffbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + def tearDown(self): + self.node.destroy_node() + + def test_node_start(self, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + # disable this test for now, as the activation of the diffbot_base_controller fails + # cnames = ["diffbot_base_controller", "joint_state_broadcaster"] + cnames = ["joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["left_wheel_joint", "right_wheel_joint"]) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @launch_testing.post_shutdown_test() +# # These tests are run after the processes in generate_test_description() have shutdown. +# class TestDescriptionCraneShutdown(unittest.TestCase): + +# def test_exit_codes(self, proc_info): +# """Check if the processes exited normally.""" +# launch_testing.asserts.assertExitCodes(proc_info) diff --git a/example_16/test/test_urdf_xacro.py b/example_16/test/test_urdf_xacro.py new file mode 100644 index 000000000..744e0e0d4 --- /dev/null +++ b/example_16/test/test_urdf_xacro.py @@ -0,0 +1,77 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Lukas Sackewitz + +import os +import shutil +import subprocess +import tempfile + +from ament_index_python.packages import get_package_share_directory + + +def test_urdf_xacro(): + # General Arguments + description_package = "ros2_control_demo_example_16" + description_file = "diffbot.urdf.xacro" + + description_file_path = os.path.join( + get_package_share_directory(description_package), "urdf", description_file + ) + + (_, tmp_urdf_output_file) = tempfile.mkstemp(suffix=".urdf") + + # Compose `xacro` and `check_urdf` command + xacro_command = ( + f"{shutil.which('xacro')}" f" {description_file_path}" f" > {tmp_urdf_output_file}" + ) + check_urdf_command = f"{shutil.which('check_urdf')} {tmp_urdf_output_file}" + + # Try to call processes but finally remove the temp file + try: + xacro_process = subprocess.run( + xacro_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert xacro_process.returncode == 0, " --- XACRO command failed ---" + + check_urdf_process = subprocess.run( + check_urdf_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert ( + check_urdf_process.returncode == 0 + ), "\n --- URDF check failed! --- \nYour xacro does not unfold into a proper urdf robot description. Please check your xacro file." + + finally: + os.remove(tmp_urdf_output_file) + + +if __name__ == "__main__": + test_urdf_xacro() diff --git a/example_16/test/test_view_robot_launch.py b/example_16/test/test_view_robot_launch.py new file mode 100644 index 000000000..bd1e87ff6 --- /dev/null +++ b/example_16/test/test_view_robot_launch.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Lukas Sackewitz + +import os +import pytest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_16"), + "launch/view_robot.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) From 66fde02b25d4f31919f11a05564f6b5e0223c6d7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 31 Jan 2025 19:04:21 +0000 Subject: [PATCH 02/10] Fix speed_limiter parameters --- .../config/diffbot_chained_controllers.yaml | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/example_16/bringup/config/diffbot_chained_controllers.yaml b/example_16/bringup/config/diffbot_chained_controllers.yaml index 063c9f2db..db0518243 100644 --- a/example_16/bringup/config/diffbot_chained_controllers.yaml +++ b/example_16/bringup/config/diffbot_chained_controllers.yaml @@ -79,24 +79,18 @@ diffbot_base_controller: # 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: 0.0 - linear.x.min_jerk: 0.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: 0.0 - angular.z.min_jerk: 0.0 + angular.z.max_jerk: .NAN + angular.z.min_jerk: .NAN forward_velocity_controller_for_debug: ros__parameters: From 96c7211b0c835aa4884e1d7e9099b5e2051d73d9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 31 Jan 2025 19:05:06 +0000 Subject: [PATCH 03/10] Spawn the two PID controllers at once --- example_16/bringup/launch/diffbot.launch.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py index d6f9cf9c5..33e94ff26 100644 --- a/example_16/bringup/launch/diffbot.launch.py +++ b/example_16/bringup/launch/diffbot.launch.py @@ -97,16 +97,15 @@ def generate_launch_description(): arguments=["joint_state_broadcaster"], ) - pid_controller_left_wheel_joint_spawner = Node( + pid_controllers_spawner = Node( package="controller_manager", executable="spawner", - arguments=["pid_controller_left_wheel_joint", "--param-file", robot_controllers], - ) - - pid_controller_right_wheel_joint_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["pid_controller_right_wheel_joint", "--param-file", robot_controllers], + arguments=[ + "pid_controller_left_wheel_joint", + "pid_controller_right_wheel_joint", + "--param-file", + robot_controllers, + ], ) robot_base_controller_spawner = Node( @@ -132,7 +131,7 @@ def generate_launch_description(): delay_robot_base_after_pid_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( - target_action=pid_controller_right_wheel_joint_spawner, + target_action=pid_controllers_spawner, on_exit=[robot_base_controller_spawner], ) ) @@ -149,8 +148,7 @@ def generate_launch_description(): nodes = [ control_node, robot_state_pub_node, - pid_controller_left_wheel_joint_spawner, - pid_controller_right_wheel_joint_spawner, + 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, From fd2332dedce8c1c0d638ce13a856f9d6332bc6ae Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 31 Jan 2025 19:05:36 +0000 Subject: [PATCH 04/10] Use PID with velocity control loop --- example_16/bringup/config/diffbot_chained_controllers.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/example_16/bringup/config/diffbot_chained_controllers.yaml b/example_16/bringup/config/diffbot_chained_controllers.yaml index db0518243..a86ac29ba 100644 --- a/example_16/bringup/config/diffbot_chained_controllers.yaml +++ b/example_16/bringup/config/diffbot_chained_controllers.yaml @@ -27,7 +27,6 @@ pid_controller_left_wheel_joint: command_interface: velocity reference_and_state_interfaces: - - position - velocity gains: @@ -43,7 +42,6 @@ pid_controller_right_wheel_joint: command_interface: velocity reference_and_state_interfaces: - - position - velocity gains: @@ -60,6 +58,9 @@ diffbot_base_controller: #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 From 164dabdb57c424752c48d8a35bb8330e9a765dd0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 31 Jan 2025 19:08:15 +0000 Subject: [PATCH 05/10] Some updates to the docs --- example_16/doc/userdoc.rst | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst index 37027a92b..1995a8b2c 100644 --- a/example_16/doc/userdoc.rst +++ b/example_16/doc/userdoc.rst @@ -8,10 +8,7 @@ DiffBot with Chained Controllers *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. -*example_16* is based of *example_2* which the hardware interface plugin is implemented having only one interface. This example demonstrates using a chaininged diff_drive_controller and pid_controller for each wheel to control the robot. The controllers are chained together to control the robot's velocity. - -* The communication is done using proprietary API to communicate with the robot control box. -* Data for all joints is exchanged at once. +*example_16* is based on *example_2*. This example demonstrates using a chained diff_drive_controller and pid_controller for each wheel to control the robot. The controllers are chained together to control the robot's twist. The *DiffBot* URDF files can be found in ``description/urdf`` folder. From 30017bd2b2a8986ce12a2ed9a141fd6e7c880c1d Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Sat, 1 Feb 2025 23:32:05 -0800 Subject: [PATCH 06/10] Update launch.py and add steps to documentation --- example_16/bringup/launch/diffbot.launch.py | 32 +++- .../description/launch/view_robot.launch.py | 8 +- example_16/doc/userdoc.rst | 137 ++++++++++++++++-- example_16/hardware/diffbot_system.cpp | 2 +- .../diffbot_system.hpp | 2 +- example_16/test/test_diffbot_launch.py | 37 ++--- 6 files changed, 166 insertions(+), 52 deletions(-) diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py index 33e94ff26..88679fa08 100644 --- a/example_16/bringup/launch/diffbot.launch.py +++ b/example_16/bringup/launch/diffbot.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 ros2_control Development Team +# 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. @@ -12,11 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +# +# Author: Julia Jia + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -28,7 +31,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "gui", - default_value="false", + default_value="true", description="Start RViz2 automatically with this launch file.", ) ) @@ -39,10 +42,18 @@ def generate_launch_description(): 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( @@ -82,12 +93,13 @@ def generate_launch_description(): output="both", parameters=[robot_description], ) + rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", - arguments=["-d", rviz_config_file], + arguments=["-d", rviz_config_file, "-f", fixed_frame_id], condition=IfCondition(gui), ) @@ -107,17 +119,21 @@ def generate_launch_description(): robot_controllers, ], ) - + + # start the base controller in inactive mode robot_base_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=[ "diffbot_base_controller", - # "forward_velocity_controller", "--param-file", robot_controllers, "--controller-ros-args", "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", + # "--inactive", + "--ros-args", + "--log-level", + "debug", ], ) @@ -136,7 +152,7 @@ def generate_launch_description(): ) ) - # Delay start of joint_state_broadcaster after `robot_controller` + # 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( diff --git a/example_16/description/launch/view_robot.launch.py b/example_16/description/launch/view_robot.launch.py index 41589297f..49f510120 100644 --- a/example_16/description/launch/view_robot.launch.py +++ b/example_16/description/launch/view_robot.launch.py @@ -1,4 +1,4 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# 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. @@ -77,7 +77,7 @@ def generate_launch_description(): ] ) robot_description = {"robot_description": robot_description_content} - + rviz_config_file = PathJoinSubstitution( [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] ) @@ -93,12 +93,14 @@ def generate_launch_description(): output="both", parameters=[robot_description], ) + + # start rviz2 with intial fixed frame id as base_link rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", - arguments=["-d", rviz_config_file], + arguments=["-d", rviz_config_file, "-f", "base_link"], condition=IfCondition(gui), ) diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst index 1995a8b2c..cd7a82423 100644 --- a/example_16/doc/userdoc.rst +++ b/example_16/doc/userdoc.rst @@ -8,18 +8,33 @@ DiffBot with Chained Controllers *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. -*example_16* is based on *example_2*. This example demonstrates using a chained diff_drive_controller and pid_controller for each wheel to control the robot. The controllers are chained together to control the robot's twist. +*example_16* extends *example_2* by demonstrating controller chaining. It shows how to chain a diff_drive_controller with two pid_controllers (one for each wheel) to achieve coordinated robot motion. The pid_controllers directly control wheel velocities, while the diff_drive_controller converts desired robot twist into wheel velocity commands. The *DiffBot* URDF files can be found in ``description/urdf`` folder. .. include:: ../../doc/run_from_docker.rst +Inspired by the scenario outlined in `ROS2 controller manager chaining documentation `__. +We'll implement a segament of the scenario and cover the chain of 'diff_drive_controller' with two PID controllers. Along with the process, we call out the pattern of contructing virtual interfaces in terms of controllers with details on what is happening behind the scenes. +Two flows are demonstrated: activation/execution flow and deactivation flow. + +In the activation and execution flow, we follow these steps: + + 1. First, we activate only the PID controllers to verify proper motor velocity control. The PID controllers accept commands through topics and provide virtual interfaces for chaining. + + 2. Next, we activate the diff_drive_controller, which connects to the PID controllers' virtual interfaces. When chained, the PID controllers switch to chained mode and disable their external command topics. This allows us to verify the differential drive kinematics. + + 3. Upon activation, the diff_drive_controller provides odometry state interfaces. + + 4. Finally, we send velocity commands to test robot movement, with dynamics enabled to demonstrate the PID controllers' behavior. + +For the deactivation flow, controllers must be deactivated in the reverse order of their chain. When a controller is deactivated, all controllers that depend on it must also be deactivated. We demonstrate this process step by step and examine the controller states at each stage. Tutorial steps -------------------------- -1. To check that *DiffBot* description is working properly use following launch commands +1. The first step is to check that *DiffBot* description is working properly use following launch commands .. code-block:: shell @@ -37,7 +52,7 @@ Tutorial steps .. code-block:: shell - ros2 launch ros2_control_demo_example_16 diffbot.launch.py + ros2 launch ros2_control_demo_example_16 diffbot.launch.py inactive_mode:=true fixed_frame_id:=base_link The launch file loads and starts the robot hardware, controllers and opens *RViz*. In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. @@ -55,19 +70,34 @@ Tutorial steps You should get .. code-block:: shell - + command interfaces - left_wheel_joint/velocity [available] [claimed] - right_wheel_joint/velocity [available] [claimed] + diffbot_base_controller/angular/velocity [unavailable] [unclaimed] + diffbot_base_controller/linear/velocity [unavailable] [unclaimed] + left_wheel_joint/velocity [available] [claimed] + pid_controller_left_wheel_joint/left_wheel_joint/velocity [available] [unclaimed] + pid_controller_right_wheel_joint/right_wheel_joint/velocity [available] [unclaimed] + right_wheel_joint/velocity [available] [claimed] state interfaces - left_wheel_joint/position - left_wheel_joint/velocity - right_wheel_joint/position - right_wheel_joint/velocity + left_wheel_joint/position + left_wheel_joint/velocity + pid_controller_left_wheel_joint/left_wheel_joint/velocity + pid_controller_right_wheel_joint/right_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. - Furthermore, we can see that the command interface is of type ``velocity``, which is typical for a differential drive robot. + In this example, diff_drive_controller/DiffDriveController is chinable after controller which another controllre can reference following command interfaces. Both intrfaces are in ``unclaimed`` state, which means that no controller is using them. + + .. code-block:: shell + command interfaces + diffbot_base_controller/angular/velocity [available] [unclaimed] + diffbot_base_controller/linear/velocity [available] [unclaimed] + + + + more, we can see that the command interface is of type ``velocity``, which is typical for a differential drive robot. 4. Check if controllers are running @@ -79,10 +109,89 @@ Tutorial steps .. code-block:: shell - diffbot_base_controller[diff_drive_controller/DiffDriveController] active - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active + diffbot_base_controller diff_drive_controller/DiffDriveController inactive + pid_controller_right_wheel_joint pid_controller/PidController active + pid_controller_left_wheel_joint pid_controller/PidController active + + +5. Activate the chained diff_drive_controller + + .. code-block:: shell + + ros2 control switch_controllers --activate diffbot_base_controller + + You should see the following output: + + .. code-block:: shell + + Successfully switched controllers + +6. Check the hardware interfaces as well as the controllers + + .. code-block:: shell + + ros2 control list_hardware_interfaces + ros2 control list_controllers + + You should see the following output: + + .. code-block:: shell + + command interfaces + diffbot_base_controller/angular/velocity [available] [unclaimed] + diffbot_base_controller/linear/velocity [available] [unclaimed] + left_wheel_joint/velocity [available] [claimed] + pid_controller_left_wheel_joint/left_wheel_joint/velocity [available] [claimed] + pid_controller_right_wheel_joint/right_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] + state interfaces + left_wheel_joint/position + left_wheel_joint/velocity + pid_controller_left_wheel_joint/left_wheel_joint/velocity + pid_controller_right_wheel_joint/right_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity + + + .. code-block:: shell + + joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active + diffbot_base_controller diff_drive_controller/DiffDriveController active + pid_controller_right_wheel_joint pid_controller/PidController active + pid_controller_left_wheel_joint pid_controller/PidController active + + + +7. Send a command to the left wheel + +.. code-block:: shell + ros2 topic pub -1 /pid_controller_left_wheel_joint/reference control_msgs/msg/MultiDOFCommand "{ + dof_names: ['left_wheel_joint/velocity'], + values: [1.0], + values_dot: [0.0] + }" + + +8. Send a command to the right wheel + + .. code-block:: shell + + ros2 topic pub -1 /pid_controller_right_wheel_joint/reference control_msgs/msg/MultiDOFCommand "{ + dof_names: ['right_wheel_joint/velocity'], + values: [1.0], + values_dot: [0.0] + }" + + +7. Change the fixed frame id for rviz to odom + + + Look for "Fixed Frame" in the "Global Options" section + Click on the frame name + Type or select the new frame ID and change it to odom then click on reset button -5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: +8. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: .. code-block:: shell diff --git a/example_16/hardware/diffbot_system.cpp b/example_16/hardware/diffbot_system.cpp index 8cae96625..95867f923 100644 --- a/example_16/hardware/diffbot_system.cpp +++ b/example_16/hardware/diffbot_system.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 ros2_control Development Team +// 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. diff --git a/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp b/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp index 0eeb99f70..495098a6e 100644 --- a/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp +++ b/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 ros2_control Development Team +// 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. diff --git a/example_16/test/test_diffbot_launch.py b/example_16/test/test_diffbot_launch.py index 6ab19d54b..be1e0a4b8 100644 --- a/example_16/test/test_diffbot_launch.py +++ b/example_16/test/test_diffbot_launch.py @@ -1,32 +1,19 @@ -# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# Copyright 2025 ros2_control Development Team # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: +# 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 # -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. +# http://www.apache.org/licenses/LICENSE-2.0 # -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the {copyright_holder} nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +# 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: Christoph Froehlich +# Author: Julia Jia import os import pytest From aea8f7841b08b38235c27c8cd358d12330ffb6e7 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Mon, 3 Feb 2025 22:14:21 -0800 Subject: [PATCH 07/10] Make a working example --- .gitignore | 2 + README.md | 4 + .../config/diffbot_chained_controllers.yaml | 8 +- example_16/bringup/launch/diffbot.launch.py | 13 +- .../description/launch/view_robot.launch.py | 7 +- example_16/doc/userdoc.rst | 283 ++++++------------ example_16/hardware/diffbot_system.cpp | 4 +- example_16/test/test_diffbot_launch.py | 10 +- 8 files changed, 116 insertions(+), 215 deletions(-) diff --git a/.gitignore b/.gitignore index 02e922e35..bb56e221f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ .ccache .work +/.vscode +*.pyc diff --git a/README.md b/README.md index fa7a539c9..8633cd050 100644 --- a/README.md +++ b/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`. diff --git a/example_16/bringup/config/diffbot_chained_controllers.yaml b/example_16/bringup/config/diffbot_chained_controllers.yaml index a86ac29ba..c73f4a9e0 100644 --- a/example_16/bringup/config/diffbot_chained_controllers.yaml +++ b/example_16/bringup/config/diffbot_chained_controllers.yaml @@ -30,8 +30,8 @@ pid_controller_left_wheel_joint: - velocity gains: - left_wheel_joint: {"p": 1.0, "i": 0.5, "d": 0.2, "i_clamp_min": -2.0, "i_clamp_max": 2.0, "antiwindup": true} - + # 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: @@ -45,8 +45,8 @@ pid_controller_right_wheel_joint: - velocity gains: - right_wheel_joint: {"p": 1.0, "i": 0.5, "d": 0.2, "i_clamp_min": -2.0, "i_clamp_max": 2.0, "antiwindup": true} - + # 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: diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py index 88679fa08..f9fb705da 100644 --- a/example_16/bringup/launch/diffbot.launch.py +++ b/example_16/bringup/launch/diffbot.launch.py @@ -12,14 +12,17 @@ # See the License for the specific language governing permissions and # limitations under the License. -# -# Author: Julia Jia from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + PathJoinSubstitution, + LaunchConfiguration, +) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -119,7 +122,7 @@ def generate_launch_description(): robot_controllers, ], ) - + # start the base controller in inactive mode robot_base_controller_spawner = Node( package="controller_manager", diff --git a/example_16/description/launch/view_robot.launch.py b/example_16/description/launch/view_robot.launch.py index 49f510120..a48efbec7 100644 --- a/example_16/description/launch/view_robot.launch.py +++ b/example_16/description/launch/view_robot.launch.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition @@ -77,7 +78,7 @@ def generate_launch_description(): ] ) robot_description = {"robot_description": robot_description_content} - + rviz_config_file = PathJoinSubstitution( [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] ) @@ -93,8 +94,8 @@ def generate_launch_description(): output="both", parameters=[robot_description], ) - - # start rviz2 with intial fixed frame id as base_link + + # start rviz2 with initial fixed frame id as base_link rviz_node = Node( package="rviz2", executable="rviz2", diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst index cd7a82423..c333d58da 100644 --- a/example_16/doc/userdoc.rst +++ b/example_16/doc/userdoc.rst @@ -8,142 +8,61 @@ DiffBot with Chained Controllers *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. -*example_16* extends *example_2* by demonstrating controller chaining. It shows how to chain a diff_drive_controller with two pid_controllers (one for each wheel) to achieve coordinated robot motion. The pid_controllers directly control wheel velocities, while the diff_drive_controller converts desired robot twist into wheel velocity commands. +This example extends *example_2* by demonstrating controller chaining with a diff_drive_controller and two pid_controllers (one for each wheel) to achieve coordinated robot motion. If you haven't already, you can find the instructions for *example_2* in :ref:`ros2_control_demos_example_2_userdoc`. It is recommended to follow the steps given in that tutorial first before proceeding with this one. + +This example demonstrates controller chaining as described in the `ROS2 controller manager chaining documentation `__. The control chain flows from the diff_drive_controller through two PID controllers to the DiffBot hardware. The diff_drive_controller converts desired robot twist into wheel velocity commands, which are then processed by the PID controllers to directly control the wheel velocities. Additionally, this example shows how to enable the feedforward mode for the PID controllers. The *DiffBot* URDF files can be found in ``description/urdf`` folder. .. include:: ../../doc/run_from_docker.rst -Inspired by the scenario outlined in `ROS2 controller manager chaining documentation `__. -We'll implement a segament of the scenario and cover the chain of 'diff_drive_controller' with two PID controllers. Along with the process, we call out the pattern of contructing virtual interfaces in terms of controllers with details on what is happening behind the scenes. - -Two flows are demonstrated: activation/execution flow and deactivation flow. - -In the activation and execution flow, we follow these steps: - - 1. First, we activate only the PID controllers to verify proper motor velocity control. The PID controllers accept commands through topics and provide virtual interfaces for chaining. - - 2. Next, we activate the diff_drive_controller, which connects to the PID controllers' virtual interfaces. When chained, the PID controllers switch to chained mode and disable their external command topics. This allows us to verify the differential drive kinematics. - - 3. Upon activation, the diff_drive_controller provides odometry state interfaces. - - 4. Finally, we send velocity commands to test robot movement, with dynamics enabled to demonstrate the PID controllers' behavior. - -For the deactivation flow, controllers must be deactivated in the reverse order of their chain. When a controller is deactivated, all controllers that depend on it must also be deactivated. We demonstrate this process step by step and examine the controller states at each stage. Tutorial steps -------------------------- -1. The first step is to check that *DiffBot* description is working properly use following launch commands +1. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with .. code-block:: shell - ros2 launch ros2_control_demo_example_16 view_robot.launch.py - - .. warning:: - Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. - This happens because ``joint_state_publisher_gui`` node need some time to start. - - .. image:: diffbot.png - :width: 400 - :alt: Differential Mobile Robot - -2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with - - .. code-block:: shell - - ros2 launch ros2_control_demo_example_16 diffbot.launch.py inactive_mode:=true fixed_frame_id:=base_link + ros2 launch ros2_control_demo_example_16 diffbot.launch.py The launch file loads and starts the robot hardware, controllers and opens *RViz*. In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. This excessive printing is only added for demonstration. In general, printing to the terminal should be avoided as much as possible in a hardware interface implementation. - If you can see an orange box in *RViz* everything has started properly. - Still, to be sure, let's introspect the control system before moving *DiffBot*. - -3. Check if the hardware interface loaded properly, by opening another terminal and executing - - .. code-block:: shell - - ros2 control list_hardware_interfaces - - You should get - - .. code-block:: shell - - command interfaces - diffbot_base_controller/angular/velocity [unavailable] [unclaimed] - diffbot_base_controller/linear/velocity [unavailable] [unclaimed] - left_wheel_joint/velocity [available] [claimed] - pid_controller_left_wheel_joint/left_wheel_joint/velocity [available] [unclaimed] - pid_controller_right_wheel_joint/right_wheel_joint/velocity [available] [unclaimed] - right_wheel_joint/velocity [available] [claimed] - state interfaces - left_wheel_joint/position - left_wheel_joint/velocity - pid_controller_left_wheel_joint/left_wheel_joint/velocity - pid_controller_right_wheel_joint/right_wheel_joint/velocity - right_wheel_joint/position - right_wheel_joint/velocity + If you can see an orange box in *RViz* everything has started properly. Let's introspect the control system before moving *DiffBot*. - The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. - - In this example, diff_drive_controller/DiffDriveController is chinable after controller which another controllre can reference following command interfaces. Both intrfaces are in ``unclaimed`` state, which means that no controller is using them. +2. Check controllers .. code-block:: shell - command interfaces - diffbot_base_controller/angular/velocity [available] [unclaimed] - diffbot_base_controller/linear/velocity [available] [unclaimed] - - - more, we can see that the command interface is of type ``velocity``, which is typical for a differential drive robot. - -4. Check if controllers are running - - .. code-block:: shell - - ros2 control list_controllers + $ ros2 control list_controllers You should get .. code-block:: shell - joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active - diffbot_base_controller diff_drive_controller/DiffDriveController inactive - pid_controller_right_wheel_joint pid_controller/PidController active - pid_controller_left_wheel_joint pid_controller/PidController active - - -5. Activate the chained diff_drive_controller - - .. code-block:: shell - - ros2 control switch_controllers --activate diffbot_base_controller - - You should see the following output: - - .. code-block:: shell - - Successfully switched controllers + joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active + diffbot_base_controller diff_drive_controller/DiffDriveController active + pid_controller_right_wheel_joint pid_controller/PidController active + pid_controller_left_wheel_joint pid_controller/PidController active -6. Check the hardware interfaces as well as the controllers +3. Check the hardware interface loaded by opening another terminal and executing .. code-block:: shell - ros2 control list_hardware_interfaces - ros2 control list_controllers + $ ros2 control list_hardware_interfaces - You should see the following output: + You should get - .. code-block:: shell + .. code-block:: shell command interfaces - diffbot_base_controller/angular/velocity [available] [unclaimed] - diffbot_base_controller/linear/velocity [available] [unclaimed] + diffbot_base_controller/angular/velocity [unavailable] [unclaimed] + diffbot_base_controller/linear/velocity [unavailable] [unclaimed] left_wheel_joint/velocity [available] [claimed] - pid_controller_left_wheel_joint/left_wheel_joint/velocity [available] [claimed] - pid_controller_right_wheel_joint/right_wheel_joint/velocity [available] [claimed] + pid_controller_left_wheel_joint/left_wheel_joint/velocity [available] [unclaimed] + pid_controller_right_wheel_joint/right_wheel_joint/velocity [available] [unclaimed] right_wheel_joint/velocity [available] [claimed] state interfaces left_wheel_joint/position @@ -154,48 +73,35 @@ Tutorial steps right_wheel_joint/velocity - .. code-block:: shell - - joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active - diffbot_base_controller diff_drive_controller/DiffDriveController active - pid_controller_right_wheel_joint pid_controller/PidController active - pid_controller_left_wheel_joint pid_controller/PidController active - + The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. There are two ``[claimed]`` interfaces from pid_controller, one for left wheel and one for right wheel. These interfaces are referenced by diff_drive_controller. By referencing them, diff_drive_controller can send commands to these interfaces. If you see these, we've successfully chained the controllers. + There are also two ``[unclaimed]`` interfaces from diff_drive_controller, one for angular velocity and one for linear velocity. These are provided by the diff_drive_controller because it is chainable. You can ignore them since we don't use them in this example. -7. Send a command to the left wheel - -.. code-block:: shell - ros2 topic pub -1 /pid_controller_left_wheel_joint/reference control_msgs/msg/MultiDOFCommand "{ - dof_names: ['left_wheel_joint/velocity'], - values: [1.0], - values_dot: [0.0] - }" +4. We specified ``feedforward_gain`` as part of ``gains`` in diffbot_chained_controllers.yaml. To actually enable feedforward mode for the pid_controller, we need to use a service provided by pid_controller. Let's enable it. + .. code-block:: shell -8. Send a command to the right wheel + $ ros2 service call /pid_controller_left_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" + $ ros2 service call /pid_controller_right_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" - .. code-block:: shell + You should get - ros2 topic pub -1 /pid_controller_right_wheel_joint/reference control_msgs/msg/MultiDOFCommand "{ - dof_names: ['right_wheel_joint/velocity'], - values: [1.0], - values_dot: [0.0] - }" + .. code-block:: shell + response: + std_srvs.srv.SetBool_Response(success=True, message='') -7. Change the fixed frame id for rviz to odom +5. To see the pid_controller in action, let's subscribe to the controler_state topic, e.g. pid_controller_left_wheel_joint/controller_state topic. + .. code-block:: shell - Look for "Fixed Frame" in the "Global Options" section - Click on the frame name - Type or select the new frame ID and change it to odom then click on reset button + $ ros2 topic echo /pid_controller_left_wheel_joint/controller_state -8. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: +6. Now we are ready to send a command to move the robot. Send a command to *Diff Drive Controller* by opening another terminal and executing .. code-block:: shell - ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " + $ ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " twist: linear: x: 0.7 @@ -206,91 +112,78 @@ Tutorial steps y: 0.0 z: 1.0" - You should now see an orange box circling in *RViz*. - Also, you should see changing states in the terminal where launch file is started. - - .. code-block:: shell - - [ros2_control_node-1] [INFO] [1721762311.808415917] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: - [ros2_control_node-1] command 43.33 for 'left_wheel_joint'! - [ros2_control_node-1] command 50.00 for 'right_wheel_joint'! - -6. Let's introspect the ros2_control hardware component. Calling + You should now see robot is moving in circles in *RViz*. - .. code-block:: shell - - ros2 control list_hardware_components - - should give you +7. In the terminal where launch file is started, you should see the commands being sent to the wheels and how they are gradually stabilizing to the target velocity. .. code-block:: shell - Hardware Component 1 - name: DiffBot - type: system - plugin name: ros2_control_demo_example_16/DiffBotSystemHardware - state: id=3 label=active - command interfaces - left_wheel_joint/velocity [available] [claimed] - right_wheel_joint/velocity [available] [claimed] + [ros2_control_node-1] [INFO] [1738648404.508385200] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 0.00 for 'right_wheel_joint/velocity'! + [ros2_control_node-1] command 0.00 for 'left_wheel_joint/velocity'! - This shows that the custom hardware interface plugin is loaded and running. If you work on a real - robot and don't have a simulator running, it is often faster to use the ``mock_components/GenericSystem`` - hardware component instead of writing a custom one. Stop the launch file and start it again with - an additional parameter + [ros2_control_node-1] [INFO] [1738648405.008399450] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 14.55 for 'right_wheel_joint/velocity'! + [ros2_control_node-1] command 13.17 for 'left_wheel_joint/velocity'! - .. code-block:: shell + [ros2_control_node-1] [INFO] [1738648405.508445448] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 49.21 for 'right_wheel_joint/velocity'! + [ros2_control_node-1] command 44.52 for 'left_wheel_joint/velocity'! - ros2 launch ros2_control_demo_example_16 diffbot.launch.py use_mock_hardware:=True + [ros2_control_node-1] [INFO] [1738648406.108246536] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 49.73 for 'right_wheel_joint/velocity'! + [ros2_control_node-1] command 43.11 for 'left_wheel_joint/velocity'! - Calling +8. Let's go back to the terminal where we subscribed to the controller_state topic and see the changing states. .. code-block:: shell - ros2 control list_hardware_components + --- + header: + stamp: + sec: 1738639255 + nanosec: 743875549 + frame_id: '' + dof_states: + - name: left_wheel_joint + reference: 0.0 + feedback: 0.0 + feedback_dot: 0.0 + error: 0.0 + error_dot: 0.0 + time_step: 0.09971356 + output: 0.0 + + --- + header: + stamp: + sec: 1738639255 + nanosec: 844169802 + frame_id: '' + dof_states: + - name: left_wheel_joint + reference: 6.3405774 + feedback: 0.0 + feedback_dot: 0.0 + error: 6.3405774 + error_dot: 0.0 + time_step: 0.100294253 + output: 7.006431313696083 - now should give you - - .. code-block:: shell - - Hardware Component 1 - name: DiffBot - type: system - plugin name: mock_components/GenericSystem - state: id=3 label=active - command interfaces - left_wheel_joint/velocity [available] [claimed] - right_wheel_joint/velocity [available] [claimed] - - You see that a different plugin was loaded. Having a look into the `diffbot.ros2_control.xacro `__, one can find the - instructions to load this plugin together with the parameter ``calculate_dynamics``. - - .. code-block:: xml - - - mock_components/GenericSystem - true - - - This enables the integration of the velocity commands to the position state interface, which can be - checked by means of ``ros2 topic echo /joint_states``: The position values are increasing over time if the robot is moving. - You now can test the setup with the commands from above, it should work identically as the custom hardware component plugin. - - More information on mock_components can be found in the :ref:`ros2_control documentation `. Files used for this demos -------------------------- -* Launch file: `diffbot.launch.py `__ -* Controllers yaml: `diffbot_controllers.yaml `__ -* URDF file: `diffbot.urdf.xacro `__ +* Launch file: `diffbot.launch.py `__ +* Controllers yaml: `diffbot_chained_controllers.yaml `__ +* URDF file: `diffbot.urdf.xacro `__ * Description: `diffbot_description.urdf.xacro `__ - * ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ + * ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ * RViz configuration: `diffbot.rviz `__ -* Hardware interface plugin: `diffbot_system.cpp `__ +* Hardware interface plugin: `diffbot_system.cpp `__ Controllers from this demo @@ -299,7 +192,3 @@ Controllers from this demo * ``Joint State Broadcaster`` (`ros2_controllers repository `__): :ref:`doc ` * ``Diff Drive Controller`` (`ros2_controllers repository `__): :ref:`doc ` * ``pid_controller`` (`ros2_controllers repository `__): :ref:`doc ` - -References --------------------------- -https://github.com/ros-controls/roscon_advanced_workshop/blob/9-chaining-controllers/solution/controlko_bringup/config/rrbot_chained_controllers.yaml diff --git a/example_16/hardware/diffbot_system.cpp b/example_16/hardware/diffbot_system.cpp index 95867f923..a5acf0ffd 100644 --- a/example_16/hardware/diffbot_system.cpp +++ b/example_16/hardware/diffbot_system.cpp @@ -201,8 +201,8 @@ hardware_interface::return_type ros2_control_demo_example_16 ::DiffBotSystemHard ss << "Writing commands:"; for (const auto & [name, descr] : joint_command_interfaces_) { - // Simulate sending commands to the hardware - set_state(name, get_command(name)); + // Simulate sending commands to the hardware with a slow down factor + set_state(name, get_command(name) * 0.8); ss << std::fixed << std::setprecision(2) << std::endl << "\t" << "command " << get_command(name) << " for '" << name << "'!"; diff --git a/example_16/test/test_diffbot_launch.py b/example_16/test/test_diffbot_launch.py index be1e0a4b8..4f17b5d06 100644 --- a/example_16/test/test_diffbot_launch.py +++ b/example_16/test/test_diffbot_launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -# -# Author: Julia Jia import os import pytest @@ -72,8 +70,12 @@ def test_node_start(self, proc_output): def test_controller_running(self, proc_output): # disable this test for now, as the activation of the diffbot_base_controller fails - # cnames = ["diffbot_base_controller", "joint_state_broadcaster"] - cnames = ["joint_state_broadcaster"] + cnames = [ + "pid_controller_left_wheel_joint", + "pid_controller_right_wheel_joint", + "diffbot_base_controller", + "joint_state_broadcaster", + ] check_controllers_running(self.node, cnames) From 64a2b6ce91c07daca61b80c01c059a71a152ab02 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Feb 2025 11:17:49 -0800 Subject: [PATCH 08/10] Update per review feedback. --- .gitignore | 2 -- example_16/bringup/launch/diffbot.launch.py | 5 ----- 2 files changed, 7 deletions(-) diff --git a/.gitignore b/.gitignore index bb56e221f..02e922e35 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,2 @@ .ccache .work -/.vscode -*.pyc diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py index f9fb705da..8552eb55a 100644 --- a/example_16/bringup/launch/diffbot.launch.py +++ b/example_16/bringup/launch/diffbot.launch.py @@ -123,7 +123,6 @@ def generate_launch_description(): ], ) - # start the base controller in inactive mode robot_base_controller_spawner = Node( package="controller_manager", executable="spawner", @@ -133,10 +132,6 @@ def generate_launch_description(): robot_controllers, "--controller-ros-args", "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", - # "--inactive", - "--ros-args", - "--log-level", - "debug", ], ) From bb45f91551b7d3e88620898180480f3cea4c688a Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Feb 2025 14:38:21 -0800 Subject: [PATCH 09/10] Update userdoc.rst --- example_16/doc/userdoc.rst | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst index c333d58da..c45aeeca4 100644 --- a/example_16/doc/userdoc.rst +++ b/example_16/doc/userdoc.rst @@ -36,7 +36,7 @@ Tutorial steps .. code-block:: shell - $ ros2 control list_controllers + ros2 control list_controllers You should get @@ -51,7 +51,7 @@ Tutorial steps .. code-block:: shell - $ ros2 control list_hardware_interfaces + ros2 control list_hardware_interfaces You should get @@ -81,8 +81,8 @@ Tutorial steps .. code-block:: shell - $ ros2 service call /pid_controller_left_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" - $ ros2 service call /pid_controller_right_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" + ros2 service call /pid_controller_left_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" \ + ros2 service call /pid_controller_right_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" You should get @@ -95,13 +95,13 @@ Tutorial steps .. code-block:: shell - $ ros2 topic echo /pid_controller_left_wheel_joint/controller_state + ros2 topic echo /pid_controller_left_wheel_joint/controller_state 6. Now we are ready to send a command to move the robot. Send a command to *Diff Drive Controller* by opening another terminal and executing .. code-block:: shell - $ ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " + ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " twist: linear: x: 0.7 From 191438fc3d51b73cd6e0ae8f49b5b25a92948636 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Feb 2025 14:41:55 -0800 Subject: [PATCH 10/10] Update userdoc.rst --- example_16/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst index c45aeeca4..2a1e89a7e 100644 --- a/example_16/doc/userdoc.rst +++ b/example_16/doc/userdoc.rst @@ -81,7 +81,7 @@ Tutorial steps .. code-block:: shell - ros2 service call /pid_controller_left_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" \ + ros2 service call /pid_controller_left_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" && \ ros2 service call /pid_controller_right_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" You should get