[Example 13] Multi-robot system with lifecycle management (#417) (#607)

* Add description for r3bot Co-authored-by: Denis Štogl <[email protected]> Co-authored-by: bailaC <[email protected]> * Remove camera links etc * Initial commit for example_13 from old PRs Co-authored-by: Denis Štogl <[email protected]> Co-authored-by: bailaC <[email protected]> * Change MD to rst syntax * Update test node syntax * Fix test nodes launch file * Enhance docs * Fix wrench config * Use global joint_state_broadcaster * Add tests * FIx links to files * Add some text * Fix dependencies * Add example13 to workflows+packages * Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota <[email protected]> * Update example_13/doc/userdoc.rst Co-authored-by: Sai Kishor Kothakota <[email protected]> * Fix rst syntax * Use new ros2controlcli verb * Add initial version for launch_test * Add a more complete test * Fix whitespaces * Fix decimal numbers Co-authored-by: Sai Kishor Kothakota <[email protected]> * Fix more decimal numbers Co-authored-by: Sai Kishor Kothakota <[email protected]> --------- Co-authored-by: Denis Štogl <[email protected]> Co-authored-by: bailaC <[email protected]> Co-authored-by: Sai Kishor Kothakota <[email protected]> Co-authored-by: Sai Kishor Kothakota <[email protected]> (cherry picked from commit d6e730f) Co-authored-by: Christoph Fröhlich <[email protected]>
ros-controls · Oct 31, 2024 · 700c5e5 · 700c5e5
1 parent 116fb21
commit 700c5e5
Show file tree

Hide file tree

Showing 19 changed files with 1,957 additions and 4 deletions.
diff --git 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 example (tba.)"
+* Example 13: ["Multi-robot system with hardware lifecycle management"](example_13)
+
+   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
@@ -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 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.
@@ -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/CMakeLists.txt b/example_13/CMakeLists.txt
@@ -0,0 +1,38 @@
+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_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
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()
diff --git a/example_13/README.md b/example_13/README.md
@@ -0,0 +1,5 @@
+# ros2_control_demo_example_13
+
+   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).
diff --git a/example_13/bringup/config/three_robots_controllers.yaml b/example_13/bringup/config/three_robots_controllers.yaml
@@ -0,0 +1,125 @@
+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
+
+# 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
+    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: rrbot_tool_link
+
+
+# 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
+    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: rrbot_with_sensor_tool_link
+
+# ThreeDofBot controllers
+threedofbot_joint_state_broadcaster:
+  ros__parameters:
+    use_local_topics: True
+    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
@@ -0,0 +1,37 @@
+rrbot_position_command_publisher:
+  ros__parameters:
+
+    publish_topic: "/rrbot_position_controller/commands"
+    wait_sec_between_publish: 5
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.785, 0.785]
+    pos2: [0.0, 0.0]
+    pos3: [-0.785, -0.785]
+    pos4: [0.0, 0.0]
+
+
+rrbot_with_sensor_position_command_publisher:
+  ros__parameters:
+
+    publish_topic: "/rrbot_with_sensor_position_controller/commands"
+    wait_sec_between_publish: 4
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.785, 0.785]
+    pos2: [0.0, 0.0]
+    pos3: [-0.785, -0.785]
+    pos4: [0.0, 0.0]
+
+
+threedofbot_position_command_publisher:
+  ros__parameters:
+
+    publish_topic: "/threedofbot_position_controller/commands"
+    wait_sec_between_publish: 3
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.785, 0.785, 0.785]
+    pos2: [0.0, 0.0, 0.0]
+    pos3: [-0.785, -0.785, -0.785]
+    pos4: [0.0, 0.0, 0.0]