From 2a7d273259e3c84d286c2e13f8fe4ea7250343c8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 01/23] Add description for r3bot MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl Co-authored-by: bailaC --- ros2_control_demo_description/CMakeLists.txt | 5 + ros2_control_demo_description/package.xml | 3 +- .../urdf/threedofbot_description.urdf.xacro | 240 ++++++++++++++++++ 3 files changed, 246 insertions(+), 2 deletions(-) create mode 100644 ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index ebe8c7f1e..bbd95eac8 100644 --- a/ros2_control_demo_description/CMakeLists.txt +++ b/ros2_control_demo_description/CMakeLists.txt @@ -16,6 +16,11 @@ install( install( DIRECTORY carlikebot/urdf carlikebot/rviz DESTINATION share/${PROJECT_NAME}/carlikebot + ) + +install( + DIRECTORY r3bot/urdf + DESTINATION share/${PROJECT_NAME}/r3bot ) install( diff --git a/ros2_control_demo_description/package.xml b/ros2_control_demo_description/package.xml index d43c784f4..5049bd128 100644 --- a/ros2_control_demo_description/package.xml +++ b/ros2_control_demo_description/package.xml @@ -6,13 +6,12 @@ Package with URDF and description files of test robots. Denis Štogl + Christoph Fröhlich Apache-2.0 ament_cmake - joint_state_publisher_gui - robot_state_publisher rviz2 diff --git a/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro b/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro new file mode 100644 index 000000000..5f9f4eac9 --- /dev/null +++ b/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro @@ -0,0 +1,240 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 50b063385a5a868953f7d5e81711efc9ba2592f5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 02/23] Remove camera links etc --- .../urdf/threedofbot_description.urdf.xacro | 74 ------------------- 1 file changed, 74 deletions(-) diff --git a/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro b/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro index 5f9f4eac9..f7df32f9d 100644 --- a/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro +++ b/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro @@ -161,80 +161,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 091d4c8f481c9523d6f7f20e2531204b07502f91 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 03/23] Initial commit for example_13 from old PRs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl Co-authored-by: bailaC --- README.md | 2 +- example_13/CMakeLists.txt | 34 ++ example_13/README.md | 328 +++++++++++++++++ .../config/three_robots_controllers.yaml | 119 ++++++ ...ee_robots_position_command_publishers.yaml | 37 ++ .../bringup/launch/three_robots.launch.py | 225 ++++++++++++ .../threedofbot.ros2_control.xacro | 42 +++ example_13/description/rviz/three_robots.rviz | 340 ++++++++++++++++++ .../description/urdf/three_robots.urdf.xacro | 66 ++++ example_13/package.xml | 37 ++ 10 files changed, 1229 insertions(+), 1 deletion(-) create mode 100644 example_13/CMakeLists.txt create mode 100644 example_13/README.md create mode 100644 example_13/bringup/config/three_robots_controllers.yaml create mode 100644 example_13/bringup/config/three_robots_position_command_publishers.yaml create mode 100644 example_13/bringup/launch/three_robots.launch.py create mode 100644 example_13/description/ros2_control/threedofbot.ros2_control.xacro create mode 100644 example_13/description/rviz/three_robots.rviz create mode 100644 example_13/description/urdf/three_robots.urdf.xacro create mode 100644 example_13/package.xml diff --git a/README.md b/README.md index 1047a7c90..bf3adcd32 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ The following examples are part of this demo repository: The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. -* Example 13: "Multi-robot example (tba.)" +* Example 13: ["Multi-robot system with lifecycle management: adjust controllers at runtime"](example_13) * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14) diff --git a/example_13/CMakeLists.txt b/example_13/CMakeLists.txt new file mode 100644 index 000000000..4241a273b --- /dev/null +++ b/example_13/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_13 LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# INSTALL +install( + DIRECTORY description/ros2_control description/urdf description/rviz + DESTINATION share/ros2_control_demo_example_13 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_13 +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_13/README.md b/example_13/README.md new file mode 100644 index 000000000..a2cbd3904 --- /dev/null +++ b/example_13/README.md @@ -0,0 +1,328 @@ +### Example XX: "Multi-robot system with tools" + +- Launch file: [three_robots.launch.py](ros2_control_demo_bringup/launch/three_robots.launch.py) +- Controllers: [three_robots_controllers.yaml](ros2_control_demo_bringup/config/three_robots_controllers.yaml) +- URDF: [three_robots.urdf.xacro](ros2_control_demo_bringup/config/three_robots.urdf.xacro) +- ros2_control URDF: [three_robots.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/three_robots.ros2_control.xacro) + + +**Hardware and interfaces**: +- RRBotSystemPositionOnly (auto-start) + - Command interfaces: + - rrbot_joint1/position + - rrbot_joint2/position + - State interfaces: + - rrbot_joint1/position + - rrbot_joint2/position + +- ExternalRRBotFTSensor (auto-start) + - State interfaces: + - rrbot_tcp_fts_sensor/force.x + - rrbot_tcp_fts_sensor/force.y + - rrbot_tcp_fts_sensor/force.z + - rrbot_tcp_fts_sensor/torque.x + - rrbot_tcp_fts_sensor/torque.y + - rrbot_tcp_fts_sensor/torque.z + +- RRBotSystemWithSensor (auto-configure) + - Command interfaces: + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + - State interfaces: + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + - rrbot_with_sensor_tcp_fts_sensor/force.x + - rrbot_with_sensor_tcp_fts_sensor/torque.z + +- ThreeDofBot + - Command interfaces + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + - State interfaces: + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + +**Available controllers**: +- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + +- RRBotSystemPositionOnly + - `rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `rrbot_position_controller[forward_command_controller/ForwardCommandController]` + +- ExternalRRBotFTSensor + - `rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` + +- RRBotSystemPositionOnly + - `rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController]` + - `rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` + +- FakeThreeDofBot + - `threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `threedofbot_position_controller[forward_command_controller/ForwardCommandController]` + - `threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController]` + + +Caveats on hardware lifecycling: +- There is currently no synchronization between available interface and controllers using them. + This means that you should stop controller before making interfaces they are using unavailable. + If you don't do this and deactivate/cleanup your interface first your computer will catch fire! +- Global Joint State Broadcaster will not broadcast interfaces that become available after it is started. + To solve this restart it manually, for now. During restart TF-transforms are not available. +- There is a possibility that hardware lifecycling (state changes) interfere with the `update`-loop if you are trying to start/stop a controller at the same time. + + +Notes: + 1. After starting the example there should be the following scene: + - right robot is moving (RRBotSystemPositionOnly - using auto-start) + - All interfaces are available and position controller is started and receives commands + - all controllers running + - left robot is standing upright (RRBotWithSensor - using auto-configure) + - only state interfaces are available therefore it can visualized, but not moved + - only position command controller is not running + - middle robot is "broken" (FakeThreeDofBot - it is only initialized) + - no interfaces are available + - all controllers inactive + + Hardware status: (`ros2 control list_hardware_components`) + + ``` + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=1 label=unconfigured + command interfaces + threedofbot_joint1/position [unavailable] [unclaimed] + threedofbot_joint1/pid_gain [unavailable] [unclaimed] + threedofbot_joint2/position [unavailable] [unclaimed] + threedofbot_joint2/pid_gain [unavailable] [unclaimed] + threedofbot_joint3/position [unavailable] [unclaimed] + threedofbot_joint3/pid_gain [unavailable] [unclaimed] + Hardware Component 2 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemWithSensorHardware + state: id=2 label=inactive + command interfaces + rrbot_with_sensor_joint1/position [available] [unclaimed] + rrbot_with_sensor_joint2/position [available] [unclaimed] + Hardware Component 3 + name: ExternalRRBotFTSensor + type: sensor + plugin name: ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + state: id=3 label=active + command interfaces + Hardware Component 4 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + rrbot_joint1/position [available] [claimed] + rrbot_joint2/position [available] [claimed] + ``` + + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + ``` + + 2. Activate `RRBotWithSensor` and its position controller. Call + ``` + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemWithSensor + target_state: + id: 0 + label: active" + ros2 control switch_controllers --activate rrbot_with_sensor_position_controller + ``` + + Scenario state: + - right robot is moving + - left robot is moving + - middle robot is "broken" + + Hardware status: `RRBotSystemWithSensor` is in state active + Controllers status, `rrbot_with_sensor_position_controller` is now active: + ``` + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + ``` + + 3. Configure `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call + ``` + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: inactive" + ros2 control switch_controllers --activate threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller + ``` + + Scenario state: + - right robot is moving + - left robot is moving + - middle robot is still "broken" + + + Hardware status: `FakeThreeDofBot` is in inactive state. + Controllers status, `threedofbot_joint_state_broadcaster` and `threedofbot_pid_gain_controller` are in active state now: + ``` + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + ``` + + 1. Restart global joint state broadcaster to broadcast all available states from the framework. + First check output to have comparison: + ``` + ros2 topic echo /joint_states --once + ``` + Restart: + ``` + ros2 control switch_controllers --deactivate joint_state_broadcaster + ros2 control switch_controllers --activate joint_state_broadcaster + ``` + Check output for comparison, now the joint_states of `threedofbot` and `rrbot_with_sensor` are broadcasted, too. + ``` + ros2 topic echo /joint_states --once + ``` + + Scenario state (everything is broken during `joint_state_broadcaster` restart): + - right robot is moving + - left robot is moving + - middle robot is now still "standing" + + 1. Activate `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call + ``` + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: active" + ros2 control switch_controllers --activate threedofbot_position_controller + ``` + + Scenario state: + - right robot is moving + - left robot is moving + - middle robot is moving + + Hardware status: `FakeThreeDofBot` is in active state. + Controllers status (all active now): + ``` + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + ``` + + 1. Deactivate `RRBotSystemPositionOnly` and its position controller (first). Call + ``` + ros2 control switch_controllers --deactivate rrbot_position_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: inactive" + ``` + + Scenario state: + - right robot is "standing" + - left robot is moving + - middle robot is moving + + Hardware status: `RRBotSystemPositionOnly` is in inactive state. + Controllers status: `rrbot_position_controller` is now in inactive state + ``` + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + ``` + + 1. Set `RRBotSystemPositionOnly` in unconfigured state, and deactivate its joint state broadcaster. + Also restart global joint state broadcaster. Call + ``` + ros2 control switch_controllers --deactivate rrbot_joint_state_broadcaster joint_state_broadcaster + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: unconfigured" + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: finalized" + ros2 control switch_controllers --activate joint_state_broadcaster + ``` + + Scenario state (everything is broken during `joint_state_broadcaster` restart): + - right robot is "broken" + - left robot is moving + - middle robot is moving + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + ``` diff --git a/example_13/bringup/config/three_robots_controllers.yaml b/example_13/bringup/config/three_robots_controllers.yaml new file mode 100644 index 000000000..fe3a2d6ca --- /dev/null +++ b/example_13/bringup/config/three_robots_controllers.yaml @@ -0,0 +1,119 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + hardware_components_initial_state: + unconfigured: + # Decide which hardware component will be only loaded + - FakeThreeDofBot + inactive: + # Decide which hardware component will start configured + - RRBotSystemWithSensor + # not listed hardware component should be started immediately + + # Global controllers + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # RRBot controllers + rrbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rrbot_position_controller: + type: forward_command_controller/ForwardCommandController + + rrbot_external_fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # RRBot with sensor controllers + rrbot_with_sensor_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rrbot_with_sensor_position_controller: + type: forward_command_controller/ForwardCommandController + + rrbot_with_sensor_fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # ThreeDofBot controllers + threedofbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + threedofbot_position_controller: + type: forward_command_controller/ForwardCommandController + + threedofbot_pid_gain_controller: + type: forward_command_controller/ForwardCommandController + + +# RRBot controllers +rrbot_joint_state_broadcaster: + ros__parameters: + joints: + - rrbot_joint1 + - rrbot_joint2 + interfaces: + - position + +rrbot_position_controller: + ros__parameters: + joints: + - rrbot_joint1 + - rrbot_joint2 + interface_name: position + +rrbot_external_fts_broadcaster: + ros__parameters: + sensor_name: rrbot_tcp_fts_sensor + frame_id: tool_link + + +# RRBot with sensor controllers +rrbot_with_sensor_joint_state_broadcaster: + ros__parameters: + joints: + - rrbot_with_sensor_joint1 + - rrbot_with_sensor_joint2 + interfaces: + - position + +rrbot_with_sensor_position_controller: + ros__parameters: + joints: + - rrbot_with_sensor_joint1 + - rrbot_with_sensor_joint2 + interface_name: position + +rrbot_with_sensor_fts_broadcaster: + ros__parameters: + interface_names.force.x: rrbot_with_sensor_tcp_fts_sensor/force.x + interface_names.torque.z: rrbot_with_sensor_tcp_fts_sensor/torque.z + frame_id: tool_link + + +# ThreeDofBot controllers +threedofbot_joint_state_broadcaster: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interfaces: + - position + - pid_gain + +threedofbot_position_controller: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interface_name: position + +threedofbot_pid_gain_controller: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interface_name: pid_gain diff --git a/example_13/bringup/config/three_robots_position_command_publishers.yaml b/example_13/bringup/config/three_robots_position_command_publishers.yaml new file mode 100644 index 000000000..512931a1f --- /dev/null +++ b/example_13/bringup/config/three_robots_position_command_publishers.yaml @@ -0,0 +1,37 @@ +rrbot_position_command_publisher: + ros__parameters: + + controller_name: "rrbot_position_controller" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] + + +rrbot_with_sensor_position_command_publisher: + ros__parameters: + + controller_name: "rrbot_with_sensor_position_controller" + wait_sec_between_publish: 4 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] + + +threedofbot_position_command_publisher: + ros__parameters: + + controller_name: "threedofbot_position_controller" + wait_sec_between_publish: 3 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785] + pos2: [0, 0, 0] + pos3: [-0.785, -0.785, -0.785] + pos4: [0, 0, 0] diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py new file mode 100644 index 000000000..c9c731974 --- /dev/null +++ b/example_13/bringup/launch/three_robots.launch.py @@ -0,0 +1,225 @@ +# Copyright (c) 2022, 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. +# +# +# Authors: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +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 = [ + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ) + ] + + # Initialize Arguments + slowdown = LaunchConfiguration("slowdown") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_13"), + "urdf", + "three_robots.urdf.xacro", + ] + ), + " slowdown:=", + slowdown, + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_13"), + "config", + "three_robots_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_13"), "rviz", "three_robots.rviz"] + ) + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_13"), + "bringup", + "config", + "three_robots_position_command_publishers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + remappings=[ + ("/joint_state_broadcaster/joint_states", "joint_states"), + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + 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], + ) + + # Separate robot state publishers for each robot + + # Global joint state broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # RRBot controllers + rrbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_joint_state_broadcaster", "-c", "/controller_manager"], + ) + rrbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_position_controller", "-c", "/controller_manager"], + ) + # External FTS broadcaster + rrbot_external_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_external_fts_broadcaster", "-c", "/controller_manager"], + ) + + # RRBot controllers + rrbot_with_sensor_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_with_sensor_joint_state_broadcaster", "-c", "/controller_manager"], + ) + rrbot_with_sensor_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "rrbot_with_sensor_position_controller", + "-c", + "/controller_manager", + "--inactive", + ], + ) + rrbot_with_sensor_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_with_sensor_fts_broadcaster", "-c", "/controller_manager"], + ) + + # ThreeDofBot controllers + threedofbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "threedofbot_joint_state_broadcaster", + "-c", + "/controller_manager", + "--inactive", + ], + ) + threedofbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["threedofbot_position_controller", "-c", "/controller_manager", "--inactive"], + ) + threedofbot_pid_gain_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--inactive"], + ) + + # Command publishers + rrbot_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + rrbot_with_sensor_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_with_sensor_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + threedofbot_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="threedofbot_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + nodes = [ + control_node, + robot_state_pub_node, + rviz_node, + joint_state_broadcaster_spawner, + rrbot_joint_state_broadcaster_spawner, + rrbot_position_controller_spawner, + rrbot_external_fts_broadcaster_spawner, + rrbot_with_sensor_joint_state_broadcaster_spawner, + rrbot_with_sensor_position_controller_spawner, + rrbot_with_sensor_fts_broadcaster_spawner, + threedofbot_joint_state_broadcaster_spawner, + threedofbot_position_controller_spawner, + threedofbot_pid_gain_controller_spawner, + rrbot_position_command_publisher, + rrbot_with_sensor_position_command_publisher, + threedofbot_position_command_publisher, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_13/description/ros2_control/threedofbot.ros2_control.xacro b/example_13/description/ros2_control/threedofbot.ros2_control.xacro new file mode 100644 index 000000000..95c072a25 --- /dev/null +++ b/example_13/description/ros2_control/threedofbot.ros2_control.xacro @@ -0,0 +1,42 @@ + + + + + + + + mock_components/GenericSystem + + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + + + diff --git a/example_13/description/rviz/three_robots.rviz b/example_13/description/rviz/three_robots.rviz new file mode 100644 index 000000000..aa589c99f --- /dev/null +++ b/example_13/description/rviz/three_robots.rviz @@ -0,0 +1,340 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1110 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + rrbot_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_with_sensor_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_with_sensor_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + threedofbot_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + threedofbot_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: All Bots + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrobt_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RRBotSystemPositionOnly + TF Prefix: rrbot_ + Update Interval: 0 + Value: false + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot_with_sensor_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RRBotWithSensor + TF Prefix: rrbot_with_sensor_ + Update Interval: 0 + Value: false + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /threedofbot_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: ThreeDofBot + TF Prefix: threedofbot_ + Update Interval: 0 + Value: false + Visual Enabled: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fts_broadcaster/wrench + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.157698154449463 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.23817427456378937 + Y: -0.0642336755990982 + Z: 2.174346685409546 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3603994846343994 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.690403461456299 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1381 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fa000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fa000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 30 diff --git a/example_13/description/urdf/three_robots.urdf.xacro b/example_13/description/urdf/three_robots.urdf.xacro new file mode 100644 index 000000000..68911d3e6 --- /dev/null +++ b/example_13/description/urdf/three_robots.urdf.xacro @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_13/package.xml b/example_13/package.xml new file mode 100644 index 000000000..6f6640f25 --- /dev/null +++ b/example_13/package.xml @@ -0,0 +1,37 @@ + + + + ros2_control_demo_example_13 + 0.0.0 + Demo package of `ros2_control` simulation with multiple robots. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Dr.-Ing. Denis Štogl + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + rrbot_description + r3bot_description + ros2_control_demo_example_4 + ros2_control_demo_example_5 + rviz2 + xacro + ros2_controllers_test_nodes + robot_state_publisher + + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + joint_state_publisher_gui + + + ament_cmake + + From b5406706b551e9899d4683421e08186c4707ef39 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 04/23] Change MD to rst syntax --- README.md | 4 +- doc/index.rst | 4 +- example_13/README.md | 329 +------------------------------ example_13/doc/userdoc.rst | 391 +++++++++++++++++++++++++++++++++++++ example_13/package.xml | 2 - 5 files changed, 400 insertions(+), 330 deletions(-) create mode 100644 example_13/doc/userdoc.rst diff --git a/README.md b/README.md index bf3adcd32..7f2b02907 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,9 @@ The following examples are part of this demo repository: The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. -* Example 13: ["Multi-robot system with lifecycle management: adjust controllers at runtime"](example_13) +* Example 13: ["Multi-robot system with lifecycle management"](example_13) + + This example shows how to include multiple robots in a single controller manager instance. * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14) diff --git a/doc/index.rst b/doc/index.rst index 9dc42afac..179f8739b 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -76,7 +76,8 @@ Example 11: "CarlikeBot" Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. -Example 13: "Multi-robot example (tba.)" +Example 13: "Multi-robot system with lifecycle management" + This example shows how to include multiple robots in a single controller manager instance. Example 14: "Modular robots with actuators not providing states and with additional sensors" The example shows how to implement robot hardware with actuators not providing states and with additional sensors. @@ -276,5 +277,6 @@ Examples Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst> Example 11: CarlikeBot <../example_11/doc/userdoc.rst> Example 12: Controller chaining <../example_12/doc/userdoc.rst> + Example 13: Multiple robots <../example_13/doc/userdoc.rst> Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst> Example 15: Using multiple controller managers <../example_15/doc/userdoc.rst> diff --git a/example_13/README.md b/example_13/README.md index a2cbd3904..1050a8583 100644 --- a/example_13/README.md +++ b/example_13/README.md @@ -1,328 +1,5 @@ -### Example XX: "Multi-robot system with tools" +# ros2_control_demo_example_13 -- Launch file: [three_robots.launch.py](ros2_control_demo_bringup/launch/three_robots.launch.py) -- Controllers: [three_robots_controllers.yaml](ros2_control_demo_bringup/config/three_robots_controllers.yaml) -- URDF: [three_robots.urdf.xacro](ros2_control_demo_bringup/config/three_robots.urdf.xacro) -- ros2_control URDF: [three_robots.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/three_robots.ros2_control.xacro) + This example shows how to include multiple robots in a single controller manager instance. - -**Hardware and interfaces**: -- RRBotSystemPositionOnly (auto-start) - - Command interfaces: - - rrbot_joint1/position - - rrbot_joint2/position - - State interfaces: - - rrbot_joint1/position - - rrbot_joint2/position - -- ExternalRRBotFTSensor (auto-start) - - State interfaces: - - rrbot_tcp_fts_sensor/force.x - - rrbot_tcp_fts_sensor/force.y - - rrbot_tcp_fts_sensor/force.z - - rrbot_tcp_fts_sensor/torque.x - - rrbot_tcp_fts_sensor/torque.y - - rrbot_tcp_fts_sensor/torque.z - -- RRBotSystemWithSensor (auto-configure) - - Command interfaces: - - rrbot_with_sensor_joint1/position - - rrbot_with_sensor_joint2/position - - State interfaces: - - rrbot_with_sensor_joint1/position - - rrbot_with_sensor_joint2/position - - rrbot_with_sensor_tcp_fts_sensor/force.x - - rrbot_with_sensor_tcp_fts_sensor/torque.z - -- ThreeDofBot - - Command interfaces - - threedofbot_joint1/position - - threedofbot_joint1/pid_gain - - threedofbot_joint2/position - - threedofbot_joint2/pid_gain - - threedofbot_joint3/position - - threedofbot_joint3/pid_gain - - State interfaces: - - threedofbot_joint1/position - - threedofbot_joint1/pid_gain - - threedofbot_joint2/position - - threedofbot_joint2/pid_gain - - threedofbot_joint3/position - - threedofbot_joint3/pid_gain - -**Available controllers**: -- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` - -- RRBotSystemPositionOnly - - `rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` - - `rrbot_position_controller[forward_command_controller/ForwardCommandController]` - -- ExternalRRBotFTSensor - - `rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` - -- RRBotSystemPositionOnly - - `rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` - - `rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController]` - - `rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` - -- FakeThreeDofBot - - `threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` - - `threedofbot_position_controller[forward_command_controller/ForwardCommandController]` - - `threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController]` - - -Caveats on hardware lifecycling: -- There is currently no synchronization between available interface and controllers using them. - This means that you should stop controller before making interfaces they are using unavailable. - If you don't do this and deactivate/cleanup your interface first your computer will catch fire! -- Global Joint State Broadcaster will not broadcast interfaces that become available after it is started. - To solve this restart it manually, for now. During restart TF-transforms are not available. -- There is a possibility that hardware lifecycling (state changes) interfere with the `update`-loop if you are trying to start/stop a controller at the same time. - - -Notes: - 1. After starting the example there should be the following scene: - - right robot is moving (RRBotSystemPositionOnly - using auto-start) - - All interfaces are available and position controller is started and receives commands - - all controllers running - - left robot is standing upright (RRBotWithSensor - using auto-configure) - - only state interfaces are available therefore it can visualized, but not moved - - only position command controller is not running - - middle robot is "broken" (FakeThreeDofBot - it is only initialized) - - no interfaces are available - - all controllers inactive - - Hardware status: (`ros2 control list_hardware_components`) - - ``` - Hardware Component 1 - name: FakeThreeDofBot - type: system - plugin name: mock_components/GenericSystem - state: id=1 label=unconfigured - command interfaces - threedofbot_joint1/position [unavailable] [unclaimed] - threedofbot_joint1/pid_gain [unavailable] [unclaimed] - threedofbot_joint2/position [unavailable] [unclaimed] - threedofbot_joint2/pid_gain [unavailable] [unclaimed] - threedofbot_joint3/position [unavailable] [unclaimed] - threedofbot_joint3/pid_gain [unavailable] [unclaimed] - Hardware Component 2 - name: RRBotSystemWithSensor - type: system - plugin name: ros2_control_demo_hardware/RRBotSystemWithSensorHardware - state: id=2 label=inactive - command interfaces - rrbot_with_sensor_joint1/position [available] [unclaimed] - rrbot_with_sensor_joint2/position [available] [unclaimed] - Hardware Component 3 - name: ExternalRRBotFTSensor - type: sensor - plugin name: ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - state: id=3 label=active - command interfaces - Hardware Component 4 - name: RRBotSystemPositionOnly - type: system - plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware - state: id=3 label=active - command interfaces - rrbot_joint1/position [available] [claimed] - rrbot_joint2/position [available] [claimed] - ``` - - Controllers status: (`ros2 control list_controllers`) - ``` - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_position_controller[forward_command_controller/ForwardCommandController] active - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive - threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive - ``` - - 2. Activate `RRBotWithSensor` and its position controller. Call - ``` - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemWithSensor - target_state: - id: 0 - label: active" - ros2 control switch_controllers --activate rrbot_with_sensor_position_controller - ``` - - Scenario state: - - right robot is moving - - left robot is moving - - middle robot is "broken" - - Hardware status: `RRBotSystemWithSensor` is in state active - Controllers status, `rrbot_with_sensor_position_controller` is now active: - ``` - $ ros2 control list_controllers - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_position_controller[forward_command_controller/ForwardCommandController] active - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive - threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive - ``` - - 3. Configure `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call - ``` - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: FakeThreeDofBot - target_state: - id: 0 - label: inactive" - ros2 control switch_controllers --activate threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller - ``` - - Scenario state: - - right robot is moving - - left robot is moving - - middle robot is still "broken" - - - Hardware status: `FakeThreeDofBot` is in inactive state. - Controllers status, `threedofbot_joint_state_broadcaster` and `threedofbot_pid_gain_controller` are in active state now: - ``` - $ ros2 control list_controllers - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_position_controller[forward_command_controller/ForwardCommandController] active - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active - threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive - ``` - - 1. Restart global joint state broadcaster to broadcast all available states from the framework. - First check output to have comparison: - ``` - ros2 topic echo /joint_states --once - ``` - Restart: - ``` - ros2 control switch_controllers --deactivate joint_state_broadcaster - ros2 control switch_controllers --activate joint_state_broadcaster - ``` - Check output for comparison, now the joint_states of `threedofbot` and `rrbot_with_sensor` are broadcasted, too. - ``` - ros2 topic echo /joint_states --once - ``` - - Scenario state (everything is broken during `joint_state_broadcaster` restart): - - right robot is moving - - left robot is moving - - middle robot is now still "standing" - - 1. Activate `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call - ``` - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: FakeThreeDofBot - target_state: - id: 0 - label: active" - ros2 control switch_controllers --activate threedofbot_position_controller - ``` - - Scenario state: - - right robot is moving - - left robot is moving - - middle robot is moving - - Hardware status: `FakeThreeDofBot` is in active state. - Controllers status (all active now): - ``` - $ ros2 control list_controllers - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_position_controller[forward_command_controller/ForwardCommandController] active - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active - threedofbot_position_controller[forward_command_controller/ForwardCommandController] active - ``` - - 1. Deactivate `RRBotSystemPositionOnly` and its position controller (first). Call - ``` - ros2 control switch_controllers --deactivate rrbot_position_controller - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemPositionOnly - target_state: - id: 0 - label: inactive" - ``` - - Scenario state: - - right robot is "standing" - - left robot is moving - - middle robot is moving - - Hardware status: `RRBotSystemPositionOnly` is in inactive state. - Controllers status: `rrbot_position_controller` is now in inactive state - ``` - $ ros2 control list_controllers - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active - threedofbot_position_controller[forward_command_controller/ForwardCommandController] active - ``` - - 1. Set `RRBotSystemPositionOnly` in unconfigured state, and deactivate its joint state broadcaster. - Also restart global joint state broadcaster. Call - ``` - ros2 control switch_controllers --deactivate rrbot_joint_state_broadcaster joint_state_broadcaster - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemPositionOnly - target_state: - id: 0 - label: unconfigured" - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemPositionOnly - target_state: - id: 0 - label: finalized" - ros2 control switch_controllers --activate joint_state_broadcaster - ``` - - Scenario state (everything is broken during `joint_state_broadcaster` restart): - - right robot is "broken" - - left robot is moving - - middle robot is moving - - Hardware status: - Controllers status: (`ros2 control list_controllers`) - ``` - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive - rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active - threedofbot_position_controller[forward_command_controller/ForwardCommandController] active - ``` +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_13/doc/userdoc.html). diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst new file mode 100644 index 000000000..91a5ec87c --- /dev/null +++ b/example_13/doc/userdoc.rst @@ -0,0 +1,391 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_13/doc/userdoc.rst + +.. _ros2_control_demos_example_13_userdoc: + +Example 13: Multi-robot system with lifecycle management +========================================================== + +- Launch file: [three_robots.launch.py](ros2_control_demo_bringup/launch/three_robots.launch.py) +- Controllers: [three_robots_controllers.yaml](ros2_control_demo_bringup/config/three_robots_controllers.yaml) +- URDF: [three_robots.urdf.xacro](ros2_control_demo_bringup/config/three_robots.urdf.xacro) +- ros2_control URDF: [three_robots.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/three_robots.ros2_control.xacro) + + +Hardware and interfaces +------------------------- + +- RRBotSystemPositionOnly (auto-start) + + - Command interfaces: + + - rrbot_joint1/position + - rrbot_joint2/position + + - State interfaces: + + - rrbot_joint1/position + - rrbot_joint2/position + +- ExternalRRBotFTSensor (auto-start) + + - State interfaces: + + - rrbot_tcp_fts_sensor/force.x + - rrbot_tcp_fts_sensor/force.y + - rrbot_tcp_fts_sensor/force.z + - rrbot_tcp_fts_sensor/torque.x + - rrbot_tcp_fts_sensor/torque.y + - rrbot_tcp_fts_sensor/torque.z + +- RRBotSystemWithSensor (auto-configure) + + - Command interfaces: + + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + + - State interfaces: + + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + - rrbot_with_sensor_tcp_fts_sensor/force.x + - rrbot_with_sensor_tcp_fts_sensor/torque.z + +- ThreeDofBot + + - Command interfaces + + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + + - State interfaces: + + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + +Available controllers +------------------------- + +- ``joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + +- RRBotSystemPositionOnly + + - ``rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + - ``rrbot_position_controller[forward_command_controller/ForwardCommandController]`` + +- ExternalRRBotFTSensor + + - ``rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`` + +- RRBotSystemPositionOnly + + - ``rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + - ``rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController]`` + - ``rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`` + +- FakeThreeDofBot + + - ``threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + - ``threedofbot_position_controller[forward_command_controller/ForwardCommandController]`` + - ``threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController]`` + + +Caveats on hardware lifecycling +-------------------------------- + +- There is currently no synchronization between available interface and controllers using them. + This means that you should stop controller before making interfaces they are using unavailable. + If you don't do this and deactivate/cleanup your interface first your computer will catch fire! +- Global Joint State Broadcaster will not broadcast interfaces that become available after it is started. + To solve this restart it manually, for now. During restart TF-transforms are not available. +- There is a possibility that hardware lifecycling (state changes) interfere with the ``update``-loop + if you are trying to start/stop a controller at the same time. + + +Tutorial steps +-------------------------- + +.. include:: ../../doc/run_from_docker.rst + +1. After starting the example there should be the following scene: + - right robot is moving (RRBotSystemPositionOnly - using auto-start) + - All interfaces are available and position controller is started and receives commands + - all controllers running + - left robot is standing upright (RRBotWithSensor - using auto-configure) + - only state interfaces are available therefore it can visualized, but not moved + - only position command controller is not running + - middle robot is "broken" (FakeThreeDofBot - it is only initialized) + - no interfaces are available + - all controllers inactive + + Hardware status: (``ros2 control list_hardware_components``) + + .. code-block:: shell + + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=1 label=unconfigured + command interfaces + threedofbot_joint1/position [unavailable] [unclaimed] + threedofbot_joint1/pid_gain [unavailable] [unclaimed] + threedofbot_joint2/position [unavailable] [unclaimed] + threedofbot_joint2/pid_gain [unavailable] [unclaimed] + threedofbot_joint3/position [unavailable] [unclaimed] + threedofbot_joint3/pid_gain [unavailable] [unclaimed] + Hardware Component 2 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemWithSensorHardware + state: id=2 label=inactive + command interfaces + rrbot_with_sensor_joint1/position [available] [unclaimed] + rrbot_with_sensor_joint2/position [available] [unclaimed] + Hardware Component 3 + name: ExternalRRBotFTSensor + type: sensor + plugin name: ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + state: id=3 label=active + command interfaces + Hardware Component 4 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + rrbot_joint1/position [available] [claimed] + rrbot_joint2/position [available] [claimed] + + Controllers status: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + + +2. Activate ``RRBotWithSensor`` and its position controller. Call + + .. code-block:: shell + + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemWithSensor + target_state: + id: 0 + label: active" + ros2 control switch_controllers --activate rrbot_with_sensor_position_controller + + Scenario state: + + - right robot is moving + - left robot is moving + - middle robot is "broken" + + Hardware status: ``RRBotSystemWithSensor`` is in state active + + Controllers status: ``rrbot_with_sensor_position_controller`` is now active: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + + +3. Configure ``FakeThreeDofBot`` and its joint state broadcaster and non-movement command interfaces. Call + + .. code-block:: shell + + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: inactive" + ros2 control switch_controllers --activate threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller + + Scenario state: + + - right robot is moving + - left robot is moving + - middle robot is still "broken" + + Hardware status: ``FakeThreeDofBot`` is in inactive state. + + Controllers status, ``threedofbot_joint_state_broadcaster`` and ``threedofbot_pid_gain_controller`` are in active state now: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + + +4. Restart global joint state broadcaster to broadcast all available states from the framework. First check output to have comparison: + + .. code-block:: shell + + ros2 topic echo /joint_states --once + + Restart: + + .. code-block:: shell + + ros2 control switch_controllers --deactivate joint_state_broadcaster + ros2 control switch_controllers --activate joint_state_broadcaster + + Check output for comparison, now the joint_states of ``threedofbot`` and ``rrbot_with_sensor`` are broadcasted, too. + + .. code-block:: shell + + ros2 topic echo /joint_states --once + + Scenario state (everything is broken during ``joint_state_broadcaster`` restart): + + - right robot is moving + - left robot is moving + - middle robot is now still "standing" + +5. Activate ``FakeThreeDofBot`` and its joint state broadcaster and non-movement command interfaces. Call + + .. code-block:: shell + + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: active" + ros2 control switch_controllers --activate threedofbot_position_controller + + Scenario state: + + - right robot is moving + - left robot is moving + - middle robot is moving + + Hardware status: ``FakeThreeDofBot`` is in active state. + + Controllers status (all active now): + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + + +6. Deactivate ``RRBotSystemPositionOnly`` and its position controller (first). Call + + .. code-block:: shell + + ros2 control switch_controllers --deactivate rrbot_position_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: inactive" + + Scenario state: + + - right robot is "standing" + - left robot is moving + - middle robot is moving + + Hardware status: ``RRBotSystemPositionOnly`` is in inactive state. + + Controllers status: ``rrbot_position_controller`` is now in inactive state + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + + +7. Set ``RRBotSystemPositionOnly`` in unconfigured state, and deactivate its joint state broadcaster. Also restart global joint state broadcaster. Call + + .. code-block:: shell + + ros2 control switch_controllers --deactivate rrbot_joint_state_broadcaster joint_state_broadcaster + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: unconfigured" + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: finalized" + ros2 control switch_controllers --activate joint_state_broadcaster + + Scenario state (everything is broken during ``joint_state_broadcaster`` restart): + + - right robot is "broken" + - left robot is moving + - middle robot is moving + + Controllers status: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active diff --git a/example_13/package.xml b/example_13/package.xml index 6f6640f25..00cc2eb39 100644 --- a/example_13/package.xml +++ b/example_13/package.xml @@ -16,8 +16,6 @@ ament_cmake - rrbot_description - r3bot_description ros2_control_demo_example_4 ros2_control_demo_example_5 rviz2 From eb7b6b828e7af8556aa1d84a7fcb0a9d9abbd303 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 05/23] Update test node syntax --- .../config/three_robots_position_command_publishers.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/example_13/bringup/config/three_robots_position_command_publishers.yaml b/example_13/bringup/config/three_robots_position_command_publishers.yaml index 512931a1f..e891fa879 100644 --- a/example_13/bringup/config/three_robots_position_command_publishers.yaml +++ b/example_13/bringup/config/three_robots_position_command_publishers.yaml @@ -1,7 +1,7 @@ rrbot_position_command_publisher: ros__parameters: - controller_name: "rrbot_position_controller" + publish_topic: "/rrbot_position_controller/commands" wait_sec_between_publish: 5 goal_names: ["pos1", "pos2", "pos3", "pos4"] @@ -14,7 +14,7 @@ rrbot_position_command_publisher: rrbot_with_sensor_position_command_publisher: ros__parameters: - controller_name: "rrbot_with_sensor_position_controller" + publish_topic: "/rrbot_with_sensor_position_controller/commands" wait_sec_between_publish: 4 goal_names: ["pos1", "pos2", "pos3", "pos4"] @@ -27,7 +27,7 @@ rrbot_with_sensor_position_command_publisher: threedofbot_position_command_publisher: ros__parameters: - controller_name: "threedofbot_position_controller" + publish_topic: "/threedofbot_position_controller/commands" wait_sec_between_publish: 3 goal_names: ["pos1", "pos2", "pos3", "pos4"] From 0b1f5d46726e9ab4f9a16e65b5b03e38bd41a2f8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 06/23] Fix test nodes launch file --- example_13/bringup/launch/three_robots.launch.py | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py index c9c731974..d30d9c4ea 100644 --- a/example_13/bringup/launch/three_robots.launch.py +++ b/example_13/bringup/launch/three_robots.launch.py @@ -68,7 +68,6 @@ def generate_launch_description(): position_goals = PathJoinSubstitution( [ FindPackageShare("ros2_control_demo_example_13"), - "bringup", "config", "three_robots_position_command_publishers.yaml", ] @@ -177,30 +176,18 @@ def generate_launch_description(): executable="publisher_forward_position_controller", name="rrbot_position_command_publisher", parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, ) rrbot_with_sensor_position_command_publisher = Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="rrbot_with_sensor_position_command_publisher", parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, ) threedofbot_position_command_publisher = Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="threedofbot_position_command_publisher", parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, ) nodes = [ From 9785fd6e5c2adb2c788b90fd636d50d3c35cf7c7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 07/23] Enhance docs --- example_13/doc/userdoc.rst | 169 +++++++++++++++++++++++++------------ 1 file changed, 116 insertions(+), 53 deletions(-) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index 91a5ec87c..5bd8fd140 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -126,60 +126,61 @@ Tutorial steps - no interfaces are available - all controllers inactive - Hardware status: (``ros2 control list_hardware_components``) + Hardware status: - .. code-block:: shell + .. code-block:: shell - Hardware Component 1 - name: FakeThreeDofBot - type: system - plugin name: mock_components/GenericSystem - state: id=1 label=unconfigured - command interfaces - threedofbot_joint1/position [unavailable] [unclaimed] - threedofbot_joint1/pid_gain [unavailable] [unclaimed] - threedofbot_joint2/position [unavailable] [unclaimed] - threedofbot_joint2/pid_gain [unavailable] [unclaimed] - threedofbot_joint3/position [unavailable] [unclaimed] - threedofbot_joint3/pid_gain [unavailable] [unclaimed] - Hardware Component 2 - name: RRBotSystemWithSensor - type: system - plugin name: ros2_control_demo_hardware/RRBotSystemWithSensorHardware - state: id=2 label=inactive - command interfaces - rrbot_with_sensor_joint1/position [available] [unclaimed] - rrbot_with_sensor_joint2/position [available] [unclaimed] - Hardware Component 3 - name: ExternalRRBotFTSensor - type: sensor - plugin name: ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - state: id=3 label=active - command interfaces - Hardware Component 4 - name: RRBotSystemPositionOnly - type: system - plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware - state: id=3 label=active - command interfaces - rrbot_joint1/position [available] [claimed] - rrbot_joint2/position [available] [claimed] + $ ros2 control list_hardware_components + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=1 label=unconfigured + command interfaces + threedofbot_joint1/position [unavailable] [unclaimed] + threedofbot_joint1/pid_gain [unavailable] [unclaimed] + threedofbot_joint2/position [unavailable] [unclaimed] + threedofbot_joint2/pid_gain [unavailable] [unclaimed] + threedofbot_joint3/position [unavailable] [unclaimed] + threedofbot_joint3/pid_gain [unavailable] [unclaimed] + Hardware Component 2 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemWithSensorHardware + state: id=2 label=inactive + command interfaces + rrbot_with_sensor_joint1/position [available] [unclaimed] + rrbot_with_sensor_joint2/position [available] [unclaimed] + Hardware Component 3 + name: ExternalRRBotFTSensor + type: sensor + plugin name: ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + state: id=3 label=active + command interfaces + Hardware Component 4 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + rrbot_joint1/position [available] [claimed] + rrbot_joint2/position [available] [claimed] Controllers status: - .. code-block:: shell + .. code-block:: shell - $ ros2 control list_controllers - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_position_controller[forward_command_controller/ForwardCommandController] active - rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active - rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive - threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive - threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive - threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive 2. Activate ``RRBotWithSensor`` and its position controller. Call @@ -199,9 +200,23 @@ Tutorial steps - left robot is moving - middle robot is "broken" - Hardware status: ``RRBotSystemWithSensor`` is in state active + Hardware status: ``RRBotSystemWithSensor`` is now in active state + + .. code-block:: shell + + $ ros2 control list_hardware_components + ... + Hardware Component 2 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_example_4/RRBotSystemWithSensorHardware + state: id=3 label=active + command interfaces + rrbot_with_sensor_joint1/position [available] [claimed] + rrbot_with_sensor_joint2/position [available] [claimed] + ... - Controllers status: ``rrbot_with_sensor_position_controller`` is now active: + Controllers status: ``rrbot_with_sensor_position_controller`` is now in active state: .. code-block:: shell @@ -233,10 +248,27 @@ Tutorial steps - right robot is moving - left robot is moving - - middle robot is still "broken" + - middle robot is standing still Hardware status: ``FakeThreeDofBot`` is in inactive state. + .. code-block:: shell + + $ ros2 control list_hardware_components + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=2 label=inactive + command interfaces + threedofbot_joint1/position [available] [unclaimed] + threedofbot_joint1/pid_gain [available] [claimed] + threedofbot_joint2/position [available] [unclaimed] + threedofbot_joint2/pid_gain [available] [claimed] + threedofbot_joint3/position [available] [unclaimed] + threedofbot_joint3/pid_gain [available] [claimed] + ... + Controllers status, ``threedofbot_joint_state_broadcaster`` and ``threedofbot_pid_gain_controller`` are in active state now: .. code-block:: shell @@ -279,7 +311,7 @@ Tutorial steps - left robot is moving - middle robot is now still "standing" -5. Activate ``FakeThreeDofBot`` and its joint state broadcaster and non-movement command interfaces. Call +5. Activate ``FakeThreeDofBot`` and its controller. Call .. code-block:: shell @@ -298,6 +330,23 @@ Tutorial steps Hardware status: ``FakeThreeDofBot`` is in active state. + .. code-block:: shell + + $ ros2 control list_hardware_components + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=3 label=active + command interfaces + threedofbot_joint1/position [available] [claimed] + threedofbot_joint1/pid_gain [available] [claimed] + threedofbot_joint2/position [available] [claimed] + threedofbot_joint2/pid_gain [available] [claimed] + threedofbot_joint3/position [available] [claimed] + threedofbot_joint3/pid_gain [available] [claimed] + ... + Controllers status (all active now): .. code-block:: shell @@ -334,6 +383,20 @@ Tutorial steps Hardware status: ``RRBotSystemPositionOnly`` is in inactive state. + .. code-block:: shell + + $ ros2 control list_hardware_components + ... + Hardware Component 4 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_example_5/RRBotSystemPositionOnlyHardware + state: id=2 label=inactive + command interfaces + rrbot_joint1/position [available] [unclaimed] + rrbot_joint2/position [available] [unclaimed] + ... + Controllers status: ``rrbot_position_controller`` is now in inactive state .. code-block:: shell @@ -370,7 +433,7 @@ Tutorial steps Scenario state (everything is broken during ``joint_state_broadcaster`` restart): - - right robot is "broken" + - right robot is standing still. - left robot is moving - middle robot is moving From 62c16ec79309dcb8192cd0b5201fdffa3e654b81 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 08/23] Fix wrench config --- .../config/three_robots_controllers.yaml | 5 +- example_13/description/rviz/three_robots.rviz | 147 ++++-------------- 2 files changed, 34 insertions(+), 118 deletions(-) diff --git a/example_13/bringup/config/three_robots_controllers.yaml b/example_13/bringup/config/three_robots_controllers.yaml index fe3a2d6ca..fb66334de 100644 --- a/example_13/bringup/config/three_robots_controllers.yaml +++ b/example_13/bringup/config/three_robots_controllers.yaml @@ -65,7 +65,7 @@ rrbot_position_controller: rrbot_external_fts_broadcaster: ros__parameters: sensor_name: rrbot_tcp_fts_sensor - frame_id: tool_link + frame_id: rrbot_tool_link # RRBot with sensor controllers @@ -88,8 +88,7 @@ rrbot_with_sensor_fts_broadcaster: ros__parameters: interface_names.force.x: rrbot_with_sensor_tcp_fts_sensor/force.x interface_names.torque.z: rrbot_with_sensor_tcp_fts_sensor/torque.z - frame_id: tool_link - + frame_id: rrbot_with_sensor_tool_link # ThreeDofBot controllers threedofbot_joint_state_broadcaster: diff --git a/example_13/description/rviz/three_robots.rviz b/example_13/description/rviz/three_robots.rviz index aa589c99f..747e1d50e 100644 --- a/example_13/description/rviz/three_robots.rviz +++ b/example_13/description/rviz/three_robots.rviz @@ -6,8 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Wrench2/Topic1 Splitter Ratio: 0.5 - Tree Height: 1110 + Tree Height: 784 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -65,20 +66,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - rrbot_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrbot_camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - rrbot_hokuyo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true rrbot_link1: Alpha: 1 Show Axes: false @@ -98,20 +85,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - rrbot_with_sensor_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrbot_with_sensor_camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - rrbot_with_sensor_hokuyo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true rrbot_with_sensor_link1: Alpha: 1 Show Axes: false @@ -131,20 +104,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - threedofbot_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - threedofbot_camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - threedofbot_hokuyo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true threedofbot_link1: Alpha: 1 Show Axes: false @@ -168,81 +127,35 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: All Bots TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rrobt_robot_description - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: RRBotSystemPositionOnly - TF Prefix: rrbot_ - Update Interval: 0 - Value: false - Visual Enabled: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rrbot_with_sensor_robot_description - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: RRBotWithSensor - TF Prefix: rrbot_with_sensor_ - Update Interval: 0 - Value: false - Visual Enabled: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /threedofbot_robot_description - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: ThreeDofBot - TF Prefix: threedofbot_ - Update Interval: 0 - Value: false - Visual Enabled: true - - Alpha: 1 + Value: /rrbot_external_fts_broadcaster/wrench + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: true + Alpha: 1 Arrow Width: 0.5 Class: rviz_default_plugins/Wrench Enabled: true @@ -253,9 +166,10 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /fts_broadcaster/wrench + Value: /rrbot_with_sensor_fts_broadcaster/wrench Torque Arrow Scale: 2 Torque Color: 204; 204; 51 Value: true @@ -274,6 +188,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -325,16 +242,16 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1381 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fa000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fa000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000003a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005c8000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 2560 - X: 0 - Y: 30 + Width: 1848 + X: 72 + Y: 27 From bfc4f7d9d95d64a5a6db97da0070c167eb510249 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 09/23] Use global joint_state_broadcaster --- example_13/bringup/config/three_robots_controllers.yaml | 7 +++++++ example_13/bringup/launch/three_robots.launch.py | 7 ------- example_13/doc/userdoc.rst | 9 +++++++-- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/example_13/bringup/config/three_robots_controllers.yaml b/example_13/bringup/config/three_robots_controllers.yaml index fb66334de..2a534cade 100644 --- a/example_13/bringup/config/three_robots_controllers.yaml +++ b/example_13/bringup/config/three_robots_controllers.yaml @@ -45,10 +45,15 @@ controller_manager: threedofbot_pid_gain_controller: type: forward_command_controller/ForwardCommandController +# global joint_state_broadcaster +# joint_state_broadcaster: +# ros__parameters: + # nothing to configure # RRBot controllers rrbot_joint_state_broadcaster: ros__parameters: + use_local_topics: True joints: - rrbot_joint1 - rrbot_joint2 @@ -71,6 +76,7 @@ rrbot_external_fts_broadcaster: # RRBot with sensor controllers rrbot_with_sensor_joint_state_broadcaster: ros__parameters: + use_local_topics: True joints: - rrbot_with_sensor_joint1 - rrbot_with_sensor_joint2 @@ -93,6 +99,7 @@ rrbot_with_sensor_fts_broadcaster: # ThreeDofBot controllers threedofbot_joint_state_broadcaster: ros__parameters: + use_local_topics: True joints: - threedofbot_joint1 - threedofbot_joint2 diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py index d30d9c4ea..7f581a8fb 100644 --- a/example_13/bringup/launch/three_robots.launch.py +++ b/example_13/bringup/launch/three_robots.launch.py @@ -77,13 +77,6 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - remappings=[ - ("/joint_state_broadcaster/joint_states", "joint_states"), - ], - output={ - "stdout": "screen", - "stderr": "screen", - }, ) robot_state_pub_node = Node( package="robot_state_publisher", diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index 5bd8fd140..13ccc59f8 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -117,12 +117,17 @@ Tutorial steps 1. After starting the example there should be the following scene: - right robot is moving (RRBotSystemPositionOnly - using auto-start) + - All interfaces are available and position controller is started and receives commands - all controllers running + - left robot is standing upright (RRBotWithSensor - using auto-configure) + - only state interfaces are available therefore it can visualized, but not moved - only position command controller is not running + - middle robot is "broken" (FakeThreeDofBot - it is only initialized) + - no interfaces are available - all controllers inactive @@ -248,7 +253,7 @@ Tutorial steps - right robot is moving - left robot is moving - - middle robot is standing still + - middle robot is still "broken" Hardware status: ``FakeThreeDofBot`` is in inactive state. @@ -377,7 +382,7 @@ Tutorial steps Scenario state: - - right robot is "standing" + - right robot is now "standing" at the last position - left robot is moving - middle robot is moving From 4e58940e7df550bf562968cb035b103858fc4075 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 10/23] Add tests --- example_13/CMakeLists.txt | 4 ++ example_13/test/test_urdf_xacro.py | 77 +++++++++++++++++++++++ example_13/test/test_view_robot_launch.py | 54 ++++++++++++++++ 3 files changed, 135 insertions(+) create mode 100644 example_13/test/test_urdf_xacro.py create mode 100644 example_13/test/test_view_robot_launch.py diff --git a/example_13/CMakeLists.txt b/example_13/CMakeLists.txt index 4241a273b..7ab813270 100644 --- a/example_13/CMakeLists.txt +++ b/example_13/CMakeLists.txt @@ -27,6 +27,10 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_13_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_13_launch test/test_view_robot_launch.py) endif() ## EXPORTS diff --git a/example_13/test/test_urdf_xacro.py b/example_13/test/test_urdf_xacro.py new file mode 100644 index 000000000..922daea13 --- /dev/null +++ b/example_13/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_13" + description_file = "three_robots.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_13/test/test_view_robot_launch.py b/example_13/test/test_view_robot_launch.py new file mode 100644 index 000000000..40795e213 --- /dev/null +++ b/example_13/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_13"), + "launch/three_robots.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) From 5364a63d95d32e4fb74a61d7195113170d0e0f2d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 11/23] FIx links to files --- example_13/doc/userdoc.rst | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index 13ccc59f8..708a0d9a2 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -5,12 +5,6 @@ Example 13: Multi-robot system with lifecycle management ========================================================== -- Launch file: [three_robots.launch.py](ros2_control_demo_bringup/launch/three_robots.launch.py) -- Controllers: [three_robots_controllers.yaml](ros2_control_demo_bringup/config/three_robots_controllers.yaml) -- URDF: [three_robots.urdf.xacro](ros2_control_demo_bringup/config/three_robots.urdf.xacro) -- ros2_control URDF: [three_robots.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/three_robots.ros2_control.xacro) - - Hardware and interfaces ------------------------- @@ -457,3 +451,19 @@ Tutorial steps threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + +Files used for this demos +------------------------- + +- Launch file: `three_robots.launch.py `__ +- Controllers yaml: `three_robots_controllers.yaml `__ +- URDF file: `three_robots.urdf.xacro `__ + + + Description: `threedofbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `three_robots.ros2_control.xacro `__ +- RViz configuration: `three_robots.rviz `__ + +Controllers from this demo +-------------------------- +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ From 99fba03d7df67e3fdee524659a08fda20f482865 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 12/23] Add some text --- example_13/doc/userdoc.rst | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index 708a0d9a2..7967ba57d 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -5,6 +5,9 @@ Example 13: Multi-robot system with lifecycle management ========================================================== +This example shows how to include multiple robots in a single controller manager instance. +Additionally, hardware lifecycle management is demonstrated. + Hardware and interfaces ------------------------- @@ -68,7 +71,9 @@ Hardware and interfaces Available controllers ------------------------- -- ``joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` +- Global + + - ``joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` - RRBotSystemPositionOnly From e12c23906709cf85c6151e25a26c823868c86afd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:34 +0000 Subject: [PATCH 13/23] Fix dependencies --- example_13/package.xml | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/example_13/package.xml b/example_13/package.xml index 00cc2eb39..349773fce 100644 --- a/example_13/package.xml +++ b/example_13/package.xml @@ -16,18 +16,25 @@ ament_cmake + controller_manager + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + robot_state_publisher ros2_control_demo_example_4 ros2_control_demo_example_5 + ros2_controllers_test_nodes + ros2controlcli + ros2launch rviz2 xacro - ros2_controllers_test_nodes - robot_state_publisher - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - joint_state_publisher_gui + ament_cmake_gtest + ament_cmake_pytest + launch_testing_ament_cmake + launch_testing_ros + liburdfdom-tools + xacro ament_cmake From 3cda15bbe3c4812bdda2933f28058b0b3bcd6581 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:52 +0000 Subject: [PATCH 14/23] Add example13 to workflows+packages --- ros2_control_demos/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index 68dae5545..32e6925c3 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -26,6 +26,7 @@ ros2_control_demo_example_10 ros2_control_demo_example_11 ros2_control_demo_example_12 + ros2_control_demo_example_13 ros2_control_demo_example_14 ros2_control_demo_example_15 From c2da0d6a8afd1d74de330e9479ab825030e2b773 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 1 Aug 2024 18:29:52 +0000 Subject: [PATCH 15/23] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- README.md | 4 ++-- doc/index.rst | 4 ++-- example_13/README.md | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 7f2b02907..ea154320d 100644 --- a/README.md +++ b/README.md @@ -66,9 +66,9 @@ The following examples are part of this demo repository: The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. -* Example 13: ["Multi-robot system with lifecycle management"](example_13) +* Example 13: ["Multi-robot system with hardware lifecycle management"](example_13) - This example shows how to include multiple robots in a single controller manager instance. + This example shows how to handle multiple robots in a single controller manager instance. * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14) diff --git a/doc/index.rst b/doc/index.rst index 179f8739b..c6fbbab02 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -76,8 +76,8 @@ Example 11: "CarlikeBot" Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. -Example 13: "Multi-robot system with lifecycle management" - This example shows how to include multiple robots in a single controller manager instance. +Example 13: "Multi-robot system with hardware lifecycle management" + This example shows how to handle multiple robots in a single controller manager instance. Example 14: "Modular robots with actuators not providing states and with additional sensors" The example shows how to implement robot hardware with actuators not providing states and with additional sensors. diff --git a/example_13/README.md b/example_13/README.md index 1050a8583..54b3d9c3a 100644 --- a/example_13/README.md +++ b/example_13/README.md @@ -1,5 +1,5 @@ # ros2_control_demo_example_13 - This example shows how to include multiple robots in a single controller manager instance. + This example shows how to handle multiple robots in a single controller manager instance. 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_13/doc/userdoc.html). From b0163ea03415a8d3f3fcad12d717734acaee3b53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 1 Aug 2024 18:29:52 +0000 Subject: [PATCH 16/23] Update example_13/doc/userdoc.rst Co-authored-by: Sai Kishor Kothakota --- example_13/doc/userdoc.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index 7967ba57d..16de25779 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -115,6 +115,11 @@ Tutorial steps .. include:: ../../doc/run_from_docker.rst 1. After starting the example there should be the following scene: + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_13 three_robots.launch.py + - right robot is moving (RRBotSystemPositionOnly - using auto-start) - All interfaces are available and position controller is started and receives commands From 10d2a52456c480e062364270d3967d79fcfb8623 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:52 +0000 Subject: [PATCH 17/23] Fix rst syntax --- example_13/doc/userdoc.rst | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index 16de25779..f83e154ee 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -114,26 +114,28 @@ Tutorial steps .. include:: ../../doc/run_from_docker.rst -1. After starting the example there should be the following scene: +1. After starting the example with - .. code-block:: shell + .. code-block:: shell ros2 launch ros2_control_demo_example_13 three_robots.launch.py - - - right robot is moving (RRBotSystemPositionOnly - using auto-start) - - All interfaces are available and position controller is started and receives commands - - all controllers running + there should be the following scene: + + - right robot is moving (RRBotSystemPositionOnly - using auto-start) + + - All interfaces are available and position controller is started and receives commands + - all controllers running - - left robot is standing upright (RRBotWithSensor - using auto-configure) + - left robot is standing upright (RRBotWithSensor - using auto-configure) - - only state interfaces are available therefore it can visualized, but not moved - - only position command controller is not running + - only state interfaces are available therefore it can visualized, but not moved + - only position command controller is not running - - middle robot is "broken" (FakeThreeDofBot - it is only initialized) + - middle robot is "broken" (FakeThreeDofBot - it is only initialized) - - no interfaces are available - - all controllers inactive + - no interfaces are available + - all controllers inactive Hardware status: From 6af32f2d968af9c89afffc4565d9fb073812358c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:52 +0000 Subject: [PATCH 18/23] Use new ros2controlcli verb --- example_13/doc/userdoc.rst | 42 +++++++++++++++----------------------- 1 file changed, 16 insertions(+), 26 deletions(-) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index f83e154ee..b09fce9b6 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -194,7 +194,13 @@ Tutorial steps threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive -2. Activate ``RRBotWithSensor`` and its position controller. Call +2. Activate ``RRBotWithSensor`` hardware component. Use either the ros2controlcli + + .. code-block:: shell + + ros2 control set_hardware_component_state RRBotSystemWithSensor active + + or call the service manually .. code-block:: shell @@ -203,6 +209,11 @@ Tutorial steps target_state: id: 0 label: active" + + Then activate its position controller via + + .. code-block:: shell + ros2 control switch_controllers --activate rrbot_with_sensor_position_controller Scenario state: @@ -248,11 +259,7 @@ Tutorial steps .. code-block:: shell - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: FakeThreeDofBot - target_state: - id: 0 - label: inactive" + ros2 control set_hardware_component_state FakeThreeDofBot inactive ros2 control switch_controllers --activate threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller Scenario state: @@ -326,11 +333,7 @@ Tutorial steps .. code-block:: shell - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: FakeThreeDofBot - target_state: - id: 0 - label: active" + ros2 control set_hardware_component_state FakeThreeDofBot active ros2 control switch_controllers --activate threedofbot_position_controller Scenario state: @@ -380,11 +383,7 @@ Tutorial steps .. code-block:: shell ros2 control switch_controllers --deactivate rrbot_position_controller - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemPositionOnly - target_state: - id: 0 - label: inactive" + ros2 control set_hardware_component_state RRBotSystemPositionOnly inactive Scenario state: @@ -430,16 +429,7 @@ Tutorial steps .. code-block:: shell ros2 control switch_controllers --deactivate rrbot_joint_state_broadcaster joint_state_broadcaster - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemPositionOnly - target_state: - id: 0 - label: unconfigured" - ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " - name: RRBotSystemPositionOnly - target_state: - id: 0 - label: finalized" + ros2 control set_hardware_component_state RRBotSystemPositionOnly unconfigured ros2 control switch_controllers --activate joint_state_broadcaster Scenario state (everything is broken during ``joint_state_broadcaster`` restart): From a9498717fc8aa01457a0a1d95fcdaf88851e282a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 1 Aug 2024 18:29:52 +0000 Subject: [PATCH 19/23] Add initial version for launch_test --- example_13/CMakeLists.txt | 2 +- .../bringup/launch/three_robots.launch.py | 34 ++-- example_13/package.xml | 4 +- example_13/test/test_three_robots_launch.py | 153 ++++++++++++++++++ 4 files changed, 178 insertions(+), 15 deletions(-) create mode 100644 example_13/test/test_three_robots_launch.py diff --git a/example_13/CMakeLists.txt b/example_13/CMakeLists.txt index 7ab813270..0f8ae8643 100644 --- a/example_13/CMakeLists.txt +++ b/example_13/CMakeLists.txt @@ -26,11 +26,11 @@ install( ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test(example_13_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_13_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_13_launch test/test_three_robots_launch.py) endif() ## EXPORTS diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py index 7f581a8fb..92a309b48 100644 --- a/example_13/bringup/launch/three_robots.launch.py +++ b/example_13/bringup/launch/three_robots.launch.py @@ -17,6 +17,7 @@ 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 @@ -25,16 +26,26 @@ def generate_launch_description(): # Declare arguments - declared_arguments = [ + declared_arguments = [] + declared_arguments.append( DeclareLaunchArgument( "slowdown", default_value="50.0", description="Slowdown factor of the RRbot.", ) - ] + ) + # Declare arguments + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) # Initialize Arguments slowdown = LaunchConfiguration("slowdown") + gui = LaunchConfiguration("gui") # Get URDF via xacro robot_description_content = Command( @@ -90,6 +101,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(gui), ) # Separate robot state publishers for each robot @@ -98,47 +110,45 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=["joint_state_broadcaster"], ) # RRBot controllers rrbot_joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["rrbot_joint_state_broadcaster", "-c", "/controller_manager"], + arguments=["rrbot_joint_state_broadcaster"], ) rrbot_position_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["rrbot_position_controller", "-c", "/controller_manager"], + arguments=["rrbot_position_controller"], ) # External FTS broadcaster rrbot_external_fts_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["rrbot_external_fts_broadcaster", "-c", "/controller_manager"], + arguments=["rrbot_external_fts_broadcaster"], ) # RRBot controllers rrbot_with_sensor_joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["rrbot_with_sensor_joint_state_broadcaster", "-c", "/controller_manager"], + arguments=["rrbot_with_sensor_joint_state_broadcaster"], ) rrbot_with_sensor_position_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=[ "rrbot_with_sensor_position_controller", - "-c", - "/controller_manager", "--inactive", ], ) rrbot_with_sensor_fts_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["rrbot_with_sensor_fts_broadcaster", "-c", "/controller_manager"], + arguments=["rrbot_with_sensor_fts_broadcaster"], ) # ThreeDofBot controllers @@ -155,12 +165,12 @@ def generate_launch_description(): threedofbot_position_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["threedofbot_position_controller", "-c", "/controller_manager", "--inactive"], + arguments=["threedofbot_position_controller", "--inactive"], ) threedofbot_pid_gain_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--inactive"], + arguments=["threedofbot_pid_gain_controller", "--inactive"], ) # Command publishers diff --git a/example_13/package.xml b/example_13/package.xml index 349773fce..0cc8f4a59 100644 --- a/example_13/package.xml +++ b/example_13/package.xml @@ -21,6 +21,7 @@ forward_command_controller joint_state_broadcaster robot_state_publisher + ros2_control_demo_description ros2_control_demo_example_4 ros2_control_demo_example_5 ros2_controllers_test_nodes @@ -29,11 +30,10 @@ rviz2 xacro - ament_cmake_gtest ament_cmake_pytest - launch_testing_ament_cmake launch_testing_ros liburdfdom-tools + ros2_control_demo_testing xacro diff --git a/example_13/test/test_three_robots_launch.py b/example_13/test/test_three_robots_launch.py new file mode 100644 index 000000000..b8c21ff2c --- /dev/null +++ b/example_13/test/test_three_robots_launch.py @@ -0,0 +1,153 @@ +# 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 +import subprocess + +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 rclpy.node import Node +from ros2_control_demo_testing.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_13"), + "launch/three_robots.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +# This is our first test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_behavior(self, proc_output): + + # --- initial configuration --- + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + ] + + check_controllers_running(self.node, cnames) + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + ], + ) + + # --- activate second robot with its controllers --- + # Command to activate the second robot with its controllers + command = [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemWithSensor", + "active", + ] + # Execute the command + subprocess.run(command) + command = [ + "ros2", + "control", + "switch_controllers", + "--activate", + "rrbot_with_sensor_position_controller", + ] + subprocess.run(command) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + ] + check_controllers_running(self.node, cnames) + # still the same joint_states + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + ], + ) + + +# 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) From fe2d9b77d5ad77760761c11b8427a30de0e3d490 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 2 Aug 2024 06:56:47 +0000 Subject: [PATCH 20/23] Add a more complete test --- example_13/test/test_three_robots_launch.py | 226 ++++++++++++++++++-- 1 file changed, 211 insertions(+), 15 deletions(-) diff --git a/example_13/test/test_three_robots_launch.py b/example_13/test/test_three_robots_launch.py index b8c21ff2c..8f7460993 100644 --- a/example_13/test/test_three_robots_launch.py +++ b/example_13/test/test_three_robots_launch.py @@ -104,23 +104,65 @@ def test_behavior(self, proc_output): # --- activate second robot with its controllers --- # Command to activate the second robot with its controllers - command = [ - "ros2", - "control", - "set_hardware_component_state", - "RRBotSystemWithSensor", - "active", - ] - # Execute the command - subprocess.run(command) - command = [ - "ros2", - "control", - "switch_controllers", - "--activate", + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemWithSensor", + "active", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "rrbot_with_sensor_position_controller", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", "rrbot_with_sensor_position_controller", ] - subprocess.run(command) + check_controllers_running(self.node, cnames) + # still the same joint_states + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + ], + ) + + # --- configure FakeThreeDofBot its controllers --- + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "FakeThreeDofBot", + "inactive", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + ] + ) cnames = [ "joint_state_broadcaster", "rrbot_external_fts_broadcaster", @@ -129,6 +171,8 @@ def test_behavior(self, proc_output): "rrbot_with_sensor_joint_state_broadcaster", "rrbot_with_sensor_fts_broadcaster", "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", ] check_controllers_running(self.node, cnames) # still the same joint_states @@ -142,6 +186,158 @@ def test_behavior(self, proc_output): ], ) + # --- restart global joint state broadcaster --- + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--deactivate", + "joint_state_broadcaster", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "joint_state_broadcaster", + ] + ) + # now the joint_states of threedofbot and rrbot_with_sensor are broadcasted, too. + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + "threedofbot_joint1", + "threedofbot_joint2", + "threedofbot_joint3", + ], + ) + + # --- activate FakeThreeDofBot and its controllers --- + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "FakeThreeDofBot", + "active", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "threedofbot_position_controller", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + "threedofbot_position_controller", + ] + check_controllers_running(self.node, cnames) + + # --- deactivate RRBotSystemPositionOnly and its controllers --- + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--deactivate", + "rrbot_position_controller", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemPositionOnly", + "inactive", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + "threedofbot_position_controller", + ] + check_controllers_running(self.node, cnames) + + # --- Set RRBotSystemPositionOnly in unconfigured state, and deactivate its joint state broadcaster. Also restart global joint state broadcaster. --- + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--deactivate", + "rrbot_joint_state_broadcaster", + "joint_state_broadcaster", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemPositionOnly", + "unconfigured", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "joint_state_broadcaster", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + "threedofbot_position_controller", + ] + check_controllers_running(self.node, cnames) + # still all joint_states are published, even if the hardware is in unconfigured state + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + "threedofbot_joint1", + "threedofbot_joint2", + "threedofbot_joint3", + ], + ) + # TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore # @launch_testing.post_shutdown_test() From c5f20f67021c874c4640721dab539096905dbc02 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 2 Aug 2024 08:08:56 +0000 Subject: [PATCH 21/23] Fix whitespaces --- ros2_control_demo_description/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index bbd95eac8..e4d1355cc 100644 --- a/ros2_control_demo_description/CMakeLists.txt +++ b/ros2_control_demo_description/CMakeLists.txt @@ -16,8 +16,8 @@ install( install( DIRECTORY carlikebot/urdf carlikebot/rviz DESTINATION share/${PROJECT_NAME}/carlikebot - ) - +) + install( DIRECTORY r3bot/urdf DESTINATION share/${PROJECT_NAME}/r3bot From 015bd3152558dd8e5fdd4d5321fb93cf8378b48d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 31 Oct 2024 10:03:25 +0100 Subject: [PATCH 22/23] Fix decimal numbers Co-authored-by: Sai Kishor Kothakota --- .../config/three_robots_position_command_publishers.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/example_13/bringup/config/three_robots_position_command_publishers.yaml b/example_13/bringup/config/three_robots_position_command_publishers.yaml index e891fa879..7c985cf65 100644 --- a/example_13/bringup/config/three_robots_position_command_publishers.yaml +++ b/example_13/bringup/config/three_robots_position_command_publishers.yaml @@ -19,9 +19,9 @@ rrbot_with_sensor_position_command_publisher: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] threedofbot_position_command_publisher: @@ -32,6 +32,6 @@ threedofbot_position_command_publisher: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785, 0.785] - pos2: [0, 0, 0] + pos2: [0.0, 0.0, 0.0] pos3: [-0.785, -0.785, -0.785] - pos4: [0, 0, 0] + pos4: [0.0, 0.0, 0.0] From 18f39a524bd8d31ae7a77d79c8c687af60ff41c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 31 Oct 2024 10:04:13 +0100 Subject: [PATCH 23/23] Fix more decimal numbers Co-authored-by: Sai Kishor Kothakota --- .../config/three_robots_position_command_publishers.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/example_13/bringup/config/three_robots_position_command_publishers.yaml b/example_13/bringup/config/three_robots_position_command_publishers.yaml index 7c985cf65..e06062d1e 100644 --- a/example_13/bringup/config/three_robots_position_command_publishers.yaml +++ b/example_13/bringup/config/three_robots_position_command_publishers.yaml @@ -6,9 +6,9 @@ rrbot_position_command_publisher: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] rrbot_with_sensor_position_command_publisher: