Initial commit from old PR

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

README.md

@@ -70,6 +70,10 @@ The following examples are part of this demo repository:
 
 * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14)
 
+* Example 15: ["Multi-robot system with namepaced controller_manager"](example_15)
+
+   This example shows how to include multiple robots in namespaced controller manager instances.
+
 ## Structure
 
 The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`.

example_15/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.16)
+project(ros2_control_demo_example_15 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_15
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/ros2_control_demo_example_15
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  ament_add_pytest_test(example_15_urdf_xacro test/test_urdf_xacro.py)
+  ament_add_pytest_test(view_example_15_launch test/test_view_robot_launch.py)
+endif()
+
+## EXPORTS
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

example_15/README.md

@@ -0,0 +1,5 @@
+# ros2_control_demo_example_15
+
+   This example shows how to include multiple robots in namespaced controller manager instances.
+
+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_15/doc/userdoc.html).

example_15/bringup/config/multi_controller_manager_forward_position_publisher.yaml

@@ -0,0 +1,25 @@
+/rrbot_1/publisher_forward_position_controller:
+  ros__parameters:
+
+
+    publish_topic: "forward_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_2/publisher_forward_position_controller:
+  ros__parameters:
+
+    publish_topic: "forward_position_controller/commands"
+    wait_sec_between_publish: 5
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [-0.785, 0.0]
+    pos2: [0.0, -0.785]
+    pos3: [+0.785, -1.57]
+    pos4: [+1.57, -0.785]

example_15/bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml

@@ -0,0 +1,50 @@
+/rrbot_1/publisher_joint_trajectory_controller:
+  ros__parameters:
+
+    controller_name: "rrbot_1/position_trajectory_controller"
+    wait_sec_between_publish: 6
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1:
+      positions: [0.785, 0.785]
+    pos2:
+      positions: [0.0, 0.0]
+    pos3:
+      positions: [-0.785, -0.785]
+    pos4:
+      positions: [0.0, 0.0]
+
+    joints:
+      - rrbot_1_joint1
+      - rrbot_1_joint2
+
+    check_starting_point: false
+    starting_point_limits:
+      joint1: [-0.1,0.1]
+      joint2: [-0.1,0.1]
+
+
+/rrbot_2/publisher_joint_trajectory_controller:
+  ros__parameters:
+
+    controller_name: "rrbot_2/position_trajectory_controller"
+    wait_sec_between_publish: 6
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1:
+      positions: [-0.785, 0.0]
+    pos2:
+      positions: [0.0, -0.785]
+    pos3:
+      positions: [+0.785, -1.57]
+    pos4:
+      positions: [+1.57, -0.785]
+
+    joints:
+      - rrbot_2_joint1
+      - rrbot_2_joint2
+
+    check_starting_point: false
+    starting_point_limits:
+      joint1: [-0.1,0.1]
+      joint2: [-0.1,0.1]

example_15/bringup/config/multi_controller_manager_rrbot_1_controllers.yaml

@@ -0,0 +1,43 @@
+/rrbot_1/controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    position_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+/rrbot_1/forward_position_controller:
+  ros__parameters:
+    joints:
+      - rrbot_1_joint1
+      - rrbot_1_joint2
+    interface_name: position
+
+
+/rrbot_1/position_trajectory_controller:
+  ros__parameters:
+    joints:
+      - rrbot_1_joint1
+      - rrbot_1_joint2
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+    state_publish_rate: 200.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+    allow_partial_joints_goal: false # Defaults to false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    constraints:
+      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
+      goal_time: 0.0 # Defaults to 0.0 (start immediately)

example_15/bringup/config/multi_controller_manager_rrbot_2_controllers.yaml

@@ -0,0 +1,43 @@
+/rrbot_2/controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    position_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+/rrbot_2/forward_position_controller:
+  ros__parameters:
+    joints:
+      - rrbot_2_joint1
+      - rrbot_2_joint2
+    interface_name: position
+
+
+/rrbot_2/position_trajectory_controller:
+  ros__parameters:
+    joints:
+      - rrbot_2_joint1
+      - rrbot_2_joint2
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+    state_publish_rate: 200.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+    allow_partial_joints_goal: false # Defaults to false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    constraints:
+      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
+      goal_time: 0.0 # Defaults to 0.0 (start immediately)

example_15/bringup/config/rrbot_namespace_controllers.yaml

@@ -0,0 +1,43 @@
+/rrbot/controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    position_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+/rrbot/forward_position_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+    interface_name: position
+
+
+/rrbot/position_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+    state_publish_rate: 200.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+    allow_partial_joints_goal: false # Defaults to false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    constraints:
+      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
+      goal_time: 0.0 # Defaults to 0.0 (start immediately)

example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml

@@ -0,0 +1,12 @@
+publisher_forward_position_controller:
+  ros__parameters:
+
+    wait_sec_between_publish: 5
+
+    publish_topic: /rrbot/forward_position_controller/commands
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.785, 0.785]
+    pos2: [0, 0]
+    pos3: [-0.785, -0.785]
+    pos4: [0, 0]

example_15/bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml

@@ -0,0 +1,24 @@
+publisher_joint_trajectory_controller:
+  ros__parameters:
+
+    controller_name: "rrbot/position_trajectory_controller"
+    wait_sec_between_publish: 6
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1:
+      positions: [0.785, 0.785]
+    pos2:
+      positions: [0.0, 0.0]
+    pos3:
+      positions: [-0.785, -0.785]
+    pos4:
+      positions: [0.0, 0.0]
+
+    joints:
+      - joint1
+      - joint2
+
+    check_starting_point: false
+    starting_point_limits:
+      joint1: [-0.1,0.1]
+      joint2: [-0.1,0.1]