Chained controllers example

README.md

@@ -656,3 +656,141 @@ Now you should also see the *RRbot* represented correctly in `RViz`.
    ```
 
 3. You should also see the *RRbot* moving in `RViz`.
+
+
+# Complex Scenarios Demos
+
+Before testing examples from this section be sure that you checked simple examples for `RRBot` and `DiffBot`.
+
+## Chained controllers
+
+Start demo:
+```
+ros2 launch ros2_control_demo_bringup rrbot_chained_controllers.launch.py
+```
+
+Load controllers:
+```
+ros2 control load_controller position_trajectory_controller
+ros2 control load_controller position_controller
+ros2 control load_controller joint1_position_controller
+ros2 control load_controller joint2_position_controller
+```
+
+Check states using:
+```
+ros2 control list_controllers
+```
+The output should be something like:
+  ```
+  joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
+  joint1_position_controller[forward_command_controller/ChainableForwardCommandController] unconfigured
+  joint2_position_controller[forward_command_controller/ChainableForwardCommandController] unconfigured
+  position_controller[forward_command_controller/ChainableForwardCommandController] unconfigured
+  position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] unconfigured
+  ```
+
+Configure all controllers:
+```
+ros2 control set_controller_state position_trajectory_controller configure
+ros2 control set_controller_state position_controller configure
+ros2 control set_controller_state joint1_position_controller configure
+ros2 control set_controller_state joint2_position_controller configure
+```
+
+Check states using:
+```
+ros2 control list_controllers
+```
+The output should be something like:
+  ```
+  joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
+  joint1_position_controller[forward_command_controller/ChainableForwardCommandController] inactive
+  joint2_position_controller[forward_command_controller/ChainableForwardCommandController] inactive
+  position_controller[forward_command_controller/ChainableForwardCommandController] inactive
+  position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive
+  ```
+
+At this stage the reference interfaces of controller are listed under `command_interfaces` when `ros2 control list_hardware_interfaces` command is executed. The output should be something like this:
+  ```
+  command interfaces
+        position_controller/joint1_position_controller/joint1/position [unclaimed]
+        position_controller/joint2_position_controller/joint2/position [unclaimed]
+        joint1/position [unclaimed]
+        joint1_position_controller/joint1/position [unclaimed]
+        joint2/position [unclaimed]
+        joint2_position_controller/joint2/position [unclaimed]
+  state interfaces
+        joint1/position
+        joint2/position
+  ```
+
+Also check required interfaces of all controllers using command `ros2 control list_controllers -v` where you can see based on interface names how controllers are chained.
+
+Now, execute the following scenario to understand how chained controllers are working.
+
+
+
+
+1. Activate `joint1_position_controller` and send command for it:
+   ```
+   ros2 control switch_controllers --start joint1_position_controller
+   ros2 topic pub /joint1_position_controller/commands std_msgs/msg/Float64MultiArray "data:
+   - 0.5"
+   ```
+   The joint1 should then move to the position.
+
+1. Activate `joint2_position_controller` and send command for it:
+   ```
+   ros2 control switch_controllers --start joint2_position_controller
+   ros2 topic pub /joint2_position_controller/commands std_msgs/msg/Float64MultiArray "data:
+   - -0.5"
+   ```
+   The `joint2` should then move to the position.
+
+Note: You can keep publishers running in a terminal so you can see the effects of controllers chaining directly.
+
+1. Activate `position_controller` and send commands to it:
+   ```
+   ros2 control switch_controllers --start position_controller
+   ```
+
+   Now, the `ros2 control list_hardware_interfaces` and `ros2 control list_controller -v` will show that `joint1_position_controller/joint1/position` and `joint2_position_controller/joint2/position` are claimed by `position_controller` controller.
+
+   Send a command to `position_controller` and check that `joint1` and `joint2` are moving to new position.
+   ```
+   ros2 topic pub /position_controller/commands std_msgs/msg/Float64MultiArray "data:
+   - 1
+   - -1"
+   ```
+
+1. Activate `position_trajectory_controller` and send commands to it:
+   ```
+   ros2 control switch_controllers --start position_trajectory_controller
+   ```
+
+   List again the interfaces to see their status changes.
+   All command interfaces are claimed now, and `position_trajectory_controller` is connecting to reference interfaces of `position_controller`.
+
+   Send some trajectories to the robot:
+   ```
+   ros2 launch ros2_control_demo_bringup test_joint_trajectory_controller.launch.py
+   ```
+
+1. Deactivate `position_trajectory_controller` and robot should either stop movement or if
+   publishers are still active robot will end up in `[1, -1]` joint states.
+   ```
+   ros2 control switch_controllers --stop position_trajectory_controller
+   ```
+
+1. Deactivate `position_controller` and robot will move to position `[0.5, -0.5]` (if publishers are still running):
+   ```
+   ros2 control switch_controllers --stop position_controller
+   ```
+
+1. Finally stop the other two controllers and check state of hardware interface and controllers.
+   Now all interfaces are "unclaimed".
+   ```
+   ros2 control switch_controllers --stop joint1_position_controller
+   ros2 control switch_controllers --stop joint2_position_controller
+   ```

ros2_control_demo_bringup/config/rrbot_chained_controllers.yaml

@@ -0,0 +1,70 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint1_position_controller:
+      type: forward_command_controller/ChainableForwardCommandController
+
+    joint2_position_controller:
+      type: forward_command_controller/ChainableForwardCommandController
+
+    position_controller:
+      type: forward_command_controller/ChainableForwardCommandController
+
+    position_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+# First-level controllers
+joint1_position_controller:
+  ros__parameters:
+    joints:
+      - joint1
+    interface_name: position
+
+
+joint2_position_controller:
+  ros__parameters:
+    joints:
+      - joint2
+    interface_name: position
+
+
+# Second-level controller
+position_controller:
+  ros__parameters:
+    joints:
+      - joint1_position_controller/joint1
+      - joint2_position_controller/joint2
+    interface_name: position
+
+
+# Third-level controller
+position_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+
+    command_joints:
+      - position_controller/joint1_position_controller/joint1
+      - position_controller/joint2_position_controller/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)

ros2_control_demo_bringup/launch/rrbot_chained_controllers.launch.py

@@ -0,0 +1,103 @@
+# Copyright 2021 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.
+
+
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("rrbot_description"),
+                    "urdf",
+                    "rrbot.urdf.xacro",
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_bringup"),
+            "config",
+            "rrbot_chained_controllers.yaml",
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("rrbot_description"), "config", "rrbot.rviz"]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        remappings=[
+            (
+                "/forward_position_controller/commands",
+                "/position_commands",
+            ),
+        ],
+        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],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+    ]
+
+    return LaunchDescription(nodes)

ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_states.ros2_control.xacro

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="rrbot_system_position_only" params="name prefix use_sim:=^|false use_fake_hardware:=^|true fake_sensor_commands:=^|false slowdown:=2.0">
+
+    <ros2_control name="${name}" type="system">
+
+      <hardware>
+        <plugin>fake_components/GenericSystem</plugin>
+        <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+        <param name="state_following_offset">0.1</param>
+      </hardware>
+
+      <joint name="joint1">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="actual_position"/>
+      </joint>
+      <joint name="joint2">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="actual_position"/>
+      </joint>
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>

ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_states.urdf.xacro

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<!-- Revolute-Revolute Manipulator -->
+<!--
+Copied and modified from ROS1 example -
+https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
+  <xacro:arg name="use_sim" default="false" />
+
+  <!-- Enable setting arguments from the launch file -->
+  <xacro:arg name="use_fake_hardware" default="true" />
+  <xacro:arg name="fake_sensor_commands" default="false" />
+  <xacro:arg name="prefix" default="" />
+  <xacro:arg name="slowdown" default="100.0" />
+
+  <!-- Import RRBot macro -->
+  <xacro:include filename="$(find rrbot_description)/urdf/rrbot_description.urdf.xacro" />
+
+  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
+  <xacro:include filename="$(find rrbot_description)/gazebo/rrbot.gazebo.xacro" />
+
+  <!-- Import Rviz colors -->
+  <xacro:include filename="$(find rrbot_description)/gazebo/rrbot.materials.xacro" />
+
+  <!-- Import RRBot ros2_control description -->
+  <xacro:include filename="$(find rrbot_description)/ros2_control/rrbot_system_multi_states.ros2_control.xacro" />
+
+  <!-- Used for fixing robot -->
+  <link name="world"/>
+  <gazebo reference="world">
+    <static>true</static>
+  </gazebo>
+
+  <xacro:rrbot parent="world" prefix="$(arg prefix)">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:rrbot>
+
+  <xacro:rrbot_gazebo prefix="$(arg prefix)" />
+
+  <xacro:rrbot_system_position_only
+    name="RRBotSystemMultiStates" prefix="$(arg prefix)"
+    use_fake_hardware="$(arg use_fake_hardware)"
+    fake_sensor_commands="$(arg fake_sensor_commands)"
+    slowdown="$(arg slowdown)" />
+
+</robot>