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

* Add description for r3bot

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>
Co-authored-by: bailaC <bailaC@users.noreply.github.com>

* Remove camera links etc

* Initial commit for example_13 from old PRs

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>
Co-authored-by: bailaC <bailaC@users.noreply.github.com>

* 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 <saisastra3@gmail.com>

* Update example_13/doc/userdoc.rst

Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>

* 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 <saisastra3@gmail.com>

* Fix more decimal numbers

Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>

---------

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>
Co-authored-by: bailaC <bailaC@users.noreply.github.com>
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
(cherry picked from commit d6e730f)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

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)
 

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>

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()

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).

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

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]