[Example 3] Moving and Restructuring (#235)

Co-authored-by: Dr. Denis <[email protected]>

README.md

@@ -73,6 +73,10 @@ Check README file inside each example folder for detailed description.
 The robot is basically a box moving according to differential drive kinematics.
 
 
+##### Example 3
+
+*RRBot* - or ''Revolute-Revolute Manipulator Robot'' with multiple interfaces
+
 ## Quick Hints
 
 These are some quick hints, especially for those coming from a ROS1 control background:
@@ -247,63 +251,6 @@ Available launch file options:
 - **TBA**
 
 
-### Example 2: "Robots with multiple interfaces"
-
-Files:
-  - Launch file: [rrbot_system_multi_interface.launch.py](ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py)
-  - Controllers yaml: [rrbot_multi_interface_forward_controllers.yaml](ros2_control_demo_bringup/config/rrbot_multi_interface_forward_controllers.yaml)
-  - URDF: [rrbot_system_multi_interface.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro)
-  - `ros2_control` URDF tag: [rrbot_system_multi_interface.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro)
-
-Interfaces:
-  - Command interfaces:
-    - joint1/position
-    - joint2/position
-    - joint1/velocity
-    - joint2/velocity
-    - joint1/acceleration
-    - joint2/acceleration
-  - State interfaces:
-    - joint1/position
-    - joint2/position
-    - joint1/velocity
-    - joint2/velocity
-    - joint1/acceleration
-    - joint2/acceleration
-
-Available controllers:
-  - `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`
-  - `forward_position_controller[position_controllers/JointGroupPositionController]`
-  - `forward_velocity_controller[velocity_controllers/JointGroupVelocityController]`
-  - `forward_acceleration_controller[forward_command_controller/ForwardCommandController]`
-  - `forward_illegal1_controller[forward_command_controller/ForwardCommandController]`
-  - `forward_illegal2_controller[forward_command_controller/ForwardCommandController]`
-
-Notes:
-  - The example shows how to implement multi-interface robot hardware taking care about interfaces used.
-    The two illegal controllers demonstrate how hardware interface declines faulty claims to access joint command interfaces.
-
-Moving the robot:
-  - when using velocity controller:
-    ```
-    ros2 topic pub /forward_velocity_controller/commands std_msgs/msg/Float64MultiArray "data:
-    - 5
-    - 5"
-    ```
-
-  - when using acceleration controller
-    ```
-    ros2 topic pub /forward_acceleration_controller/commands std_msgs/msg/Float64MultiArray "data:
-    - 10
-    - 10"
-    ```
-
-Useful launch-file options:
-  - `robot_controller:=forward_position_controller` - starts demo and spawns position controller.
-    Robot can be then controlled using `forward_position_controller` as described below.
-  - `robot_controller:=forward_acceleration_controller` - starts demo and spawns acceleration controller.
-    Robot can be then controlled using `forward_acceleration_controller` as described below.
-
 
 ### Example 3: "Industrial robot with integrated sensor"
 

example_3/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2_control_demo_example_3)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+
+## COMPILE
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  hardware/rrbot_system_multi_interface.cpp
+)
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  hardware/include
+)
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_3_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_3.xml)
+
+# INSTALL
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include
+)
+install(
+  DIRECTORY description/launch description/ros2_control description/urdf description/rviz description/meshes
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+ament_package()

example_3/README.rst

@@ -0,0 +1,185 @@
+************************************************
+Example 3: Robots with multiple interfaces
+************************************************
+
+The example shows how to implement multi-interface robot hardware taking care about interfaces used.
+The two illegal controllers demonstrate how hardware interface declines faulty claims to access joint command interfaces.
+
+1. To check that *RRBot* descriptions are working properly use following launch commands
+
+   .. code-block:: shell
+
+    ros2 launch ros2_control_demo_example_3 view_robot.launch.py
+
+   **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``.
+   This happens because ``joint_state_publisher_gui`` node need some time to start.
+   The ``joint_state_publisher_gui`` provides a GUI to generate  a random configuration for rrbot. It is immediately displayed in *RViz*.
+
+
+2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with
+
+   .. code-block:: shell
+
+    ros2 launch ros2_control_demo_example_3 rrbot_system_multi_interface.launch.py
+
+   Useful launch-file options:
+    - ``robot_controller:=forward_position_controller`` - starts demo and spawns position controller.
+      Robot can be then controlled using ``forward_position_controller`` as described below.
+    - ``robot_controller:=forward_acceleration_controller`` - starts demo and spawns acceleration controller.
+      Robot can be then controlled using ``forward_acceleration_controller`` as described below.
+
+   The launch file loads and starts the robot hardware, controllers and opens *RViz*.
+   In starting terminal you will see a lot of output from the hardware implementation showing its internal states.
+   This is only of exemplary purposes and should be avoided as much as possible in a hardware interface implementation.
+
+   If you can see two orange and one yellow rectangle in in *RViz* everything has started properly.
+   Still, to be sure, let's introspect the control system before moving *RRBot*.
+
+3. Check if the hardware interface loaded properly, by opening another terminal and executing
+
+   .. code-block:: shell
+
+    ros2 control list_hardware_interfaces
+
+   .. code-block:: shell
+
+    command interfaces
+        joint1/acceleration [available] [unclaimed]
+        joint1/position [available] [unclaimed]
+        joint1/velocity [available] [claimed]
+        joint2/acceleration [available] [unclaimed]
+        joint2/position [available] [unclaimed]
+        joint2/velocity [available] [claimed]
+    state interfaces
+        joint1/acceleration
+        joint1/position
+        joint1/velocity
+        joint2/acceleration
+        joint2/position
+        joint2/velocity
+
+   Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*.
+
+4. Check which controllers are running
+
+   .. code-block:: shell
+
+    ros2 control list_controllers
+
+   gives
+
+   .. code-block:: shell
+
+    joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
+    forward_velocity_controller[velocity_controllers/JointGroupVelocityController] active
+
+   Check how this output changes if you use the different launch file arguments described above.
+
+5. If you get output from above you can send commands to *Forward Command Controller*, either:
+
+   #. Manually using ros2 cli interface.
+
+      * when using ``forward_position_controller`` controller
+
+        .. code-block:: shell
+
+          ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
+          - 0.5
+          - 0.5"
+
+      * when using ``forward_velocity_controller`` controller (default)
+
+        .. code-block:: shell
+
+          ros2 topic pub /forward_velocity_controller/commands std_msgs/msg/Float64MultiArray "data:
+          - 5
+          - 5"
+
+      * when using ``forward_acceleration_controller`` controller
+
+        .. code-block:: shell
+
+          ros2 topic pub /forward_acceleration_controller/commands std_msgs/msg/Float64MultiArray "data:
+          - 10
+          - 10"
+
+
+   #. Or you can start a demo node which sends two goals every 5 seconds in a loop when using ``forward_position_controller`` controller
+
+      .. code-block:: shell
+
+         ros2 launch ros2_control_demo_example_3 test_forward_position_controller.launch.py
+
+   You should now see orange and yellow blocks moving in *RViz*.
+   Also, you should see changing states in the terminal where launch file is started, e.g.
+
+   .. code-block:: shell
+
+      [RRBotSystemMultiInterfaceHardware]: Got the commands pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 0, control_lvl:1
+      [RRBotSystemMultiInterfaceHardware]: Got the commands pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 1, control_lvl:1
+      [RRBotSystemMultiInterfaceHardware]: Got pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 0!
+      [RRBotSystemMultiInterfaceHardware]: Got pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 1!
+
+6. To demonstrate illegal controller configuration, use one of the following launch file arguments:
+
+   - ``robot_controller:=forward_illegal1_controller`` or
+   - ``robot_controller:=forward_illegal2_controller``
+
+   You will see the following error messages
+
+   .. code-block:: shell
+
+    [ros2_control_node-1] [ERROR] [1676209982.531163501] [resource_manager]: Component 'RRBotSystemMultiInterface' did not accept new command resource combination:
+    [ros2_control_node-1]  Start interfaces:
+    [ros2_control_node-1] [
+    [ros2_control_node-1]   joint1/position
+    [ros2_control_node-1] ]
+    [ros2_control_node-1] Stop interfaces:
+    [ros2_control_node-1] [
+    [ros2_control_node-1] ]
+    [ros2_control_node-1]
+    [ros2_control_node-1] [ERROR] [1676209982.531223835] [controller_manager]: Could not switch controllers since prepare command mode switch was rejected.
+    [spawner-4] [ERROR] [1676209982.531717376] [spawner_forward_illegal1_controller]: Failed to activate controller
+
+   Running ``ros2 control list_hardware_interfaces`` shows that no interface is claimed
+
+   .. code-block:: shell
+
+    command interfaces
+          joint1/acceleration [available] [unclaimed]
+          joint1/position [available] [unclaimed]
+          joint1/velocity [available] [unclaimed]
+          joint2/acceleration [available] [unclaimed]
+          joint2/position [available] [unclaimed]
+          joint2/velocity [available] [unclaimed]
+    state interfaces
+          joint1/acceleration
+          joint1/position
+          joint1/velocity
+          joint2/acceleration
+          joint2/position
+          joint2/velocity
+
+   and ``ros2 control list_controllers`` indicates that the illegal controller was not loaded
+
+   .. code-block:: shell
+
+    joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
+    forward_illegal1_controller[forward_command_controller/ForwardCommandController] inactive
+
+Files used for this demos
+#########################
+
+- Launch file: `rrbot_system_multi_interface.launch.py <https://github.com/ros-controls/ros2_control_demos/example_3/bringup/launch/rrbot_system_multi_interface.launch.py>`__
+- Controllers yaml: `rrbot_multi_interface_forward_controllers.yaml <https://github.com/ros-controls/ros2_control_demos/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml>`__
+- URDF: `rrbot_system_multi_interface.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/example_3/description/urdf/rrbot_system_multi_interface.urdf.xacro>`__
+
+  + ``ros2_control`` URDF tag: `rrbot_system_multi_interface.ros2_control.xacro <https://github.com/ros-controls/ros2_control_demos/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro>`__
+
+- RViz configuration: `rrbot.rviz <https://github.com/ros-controls/ros2_control_demos/example_3/description/rviz/rrbot.rviz>`__
+- Hardware interface plugin: `rrbot_system_multi_interface.cpp <https://github.com/ros-controls/ros2_control_demos/example_3/hardware/rrbot_system_multi_interface.urdf.xacro>`__
+
+Controllers from this demo
+##########################
+- ``Joint State Broadcaster`` (`ros2_controllers repository <https://github.com/ros-controls/ros2_controllers>`__): `doc <https://control.ros.org/master/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html>`__
+- ``Forward Command Controller`` (`ros2_controllers repository <https://github.com/ros-controls/ros2_controllers>`__): `doc <https://control.ros.org/master/doc/ros2_controllers/forward_command_controller/doc/userdoc.html>`__

example_3/bringup/config/rrbot_forward_position_publisher.yaml

@@ -0,0 +1,11 @@
+publisher_forward_position_controller:
+  ros__parameters:
+
+    wait_sec_between_publish: 5
+    publish_topic: "/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]