[Example 6] Moving and Restructuring (#238)

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

README.md

@@ -77,6 +77,10 @@ The robot is basically a box moving according to differential drive kinematics.
 
 *RRBot* - or ''Revolute-Revolute Manipulator Robot'' with multiple interfaces
 
+##### Example 6: "Modular Robots with separate communication to each actuator"
+
+The example shows how to implement robot hardware with separate communication to each actuator.
+
 ## Quick Hints
 
 These are some quick hints, especially for those coming from a ROS1 control background:
@@ -261,27 +265,6 @@ Available launch file options:
 
 
 
-### Example 5: "Modular Robots with separate communication to each actuator"
-
-- Launch file: [rrbot_modular_actuators.launch.py](ros2_control_demo_bringup/launch/rrbot_modular_actuators.launch.py)
-- Controllers: [rrbot_modular_actuators.yaml](ros2_control_demo_bringup/config/rrbot_modular_actuators.yaml)
-- URDF: [rrbot_modular_actuators.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators.urdf.xacro)
-- ros2_control URDF: [rrbot_modular_actuators.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro)
-
-- Command interfaces:
-  - joint1/position
-  - joint2/position
-- State interfaces:
-  - joint1/position
-  - joint2/position
-
-Available controllers:
-- `forward_position_controller[forward_command_controller/ForwardCommandController]`
-- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`
-
-Commanding the robot: see the commands below.
-
-
 ### Example 6: "Industrial Robots with an exposed transmission interface"
 
 Files:

example_6/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2_control_demo_example_6)
+
+# 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_actuator.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_6_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_6.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_6/README.rst

@@ -0,0 +1,105 @@
+***********************************************************************
+Example 6: Modular Robots with separate communication to each actuator
+***********************************************************************
+
+The example shows how to implement robot hardware with separate communication to each actuator. This
+is implemented with a hardware interface of type ``hardware_interface::ActuatorInterface``.
+
+1. To check that *RRBot* descriptions are working properly use following launch commands
+
+   .. code-block:: shell
+
+    ros2 launch ros2_control_demo_example_6 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_6 rrbot.launch.py
+
+   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/position [available] [claimed]
+        joint2/position [available] [claimed]
+      state interfaces
+        joint1/position
+        joint2/position
+
+   Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*.
+
+4. Check is controllers are running
+
+   .. code-block:: shell
+
+    ros2 control list_controllers
+
+   .. code-block:: shell
+
+    forward_position_controller[forward_command_controller/ForwardCommandController]
+    joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]
+
+5. If you get output from above you can send commands to *Forward Command Controller*, either:
+
+   #. Manually using ros2 cli interface.
+
+      .. code-block:: shell
+
+        ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
+        - 0.5
+        - 0.5"
+
+   #. Or you can start a demo node which sends two goals every 5 seconds in a loop
+
+      .. code-block:: shell
+
+        ros2 launch ros2_control_demo_example_6 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
+
+    [RRBotModularJoint]: Writing...please wait...
+    [RRBotModularJoint]: Got command 0.50000 for joint 'joint1'!
+    [RRBotModularJoint]: Joints successfully written!
+    [RRBotModularJoint]: Writing...please wait...
+    [RRBotModularJoint]: Got command 0.50000 for joint 'joint2'!
+    [RRBotModularJoint]: Joints successfully written!
+
+
+Files used for this demos
+#########################
+
+- Launch file: `rrbot_modular_actuators.launch.py <https://github.com/ros-controls/ros2_control_demos/example_6/bringup/launch/rrbot_modular_actuators.launch.py>`__
+- Controllers yaml: `rrbot_modular_actuators.yaml <https://github.com/ros-controls/ros2_control_demos/example_6/bringup/config/rrbot_modular_actuators.yaml>`__
+- URDF: `rrbot_modular_actuators.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/example_6/description/urdf/rrbot_modular_actuators.urdf.xacro>`__
+
+  + ``ros2_control`` URDF tag: `rrbot_modular_actuators.ros2_control.xacro <https://github.com/ros-controls/ros2_control_demos/example_6/description/ros2_control/rrbot_modular_actuators.ros2_control.xacro>`__
+
+- RViz configuration: `rrbot.rviz <https://github.com/ros-controls/ros2_control_demos/example_6/description/rviz/rrbot.rviz>`__
+
+- Hardware interface plugin: `rrbot_modular_actuators.cpp <https://github.com/ros-controls/ros2_control_demos/example_6/hardware/rrbot_modular_actuators.cpp>`__
+
+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_6/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]