Initial commit for example_13 from old PRs

Co-authored-by: Denis Štogl <[email protected]>
Co-authored-by: bailaC <[email protected]>

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)
 

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

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

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]