[Example 4] Moving and Restructuring (#236)

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

Author: christophfroehlich

README.md

@@ -72,9 +72,6 @@ Check README file inside each example folder for detailed description.
 *DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive.
 The robot is basically a box moving according to differential drive kinematics.
 
-##### Example 5: "Industrial Robots with externally connected sensor"
-*RRBot* - or ''Revolute-Revolute Manipulator Robot'' - a simple position controlled robot
-with an externally connected sensor.
 
 ##### Example 3
 
@@ -97,6 +94,14 @@ These are some quick hints, especially for those coming from a ROS1 control back
 * Joint names in <ros2_control> tags in the URDF must be compatible with the controller's configuration.
 
 
+##### Example 4: "Industrial robot with integrated sensor"
+*RRBot* - or ''Revolute-Revolute Manipulator Robot'' - a simple position controlled robot with an integrated sensor.
+
+
+##### Example 5: "Industrial Robots with externally connected sensor"
+*RRBot* - or ''Revolute-Revolute Manipulator Robot'' - a simple position controlled robot with an externally connected sensor.
+
+
 ## Build status
 
 ROS2 Distro | Branch | Build status | Documentation
@@ -255,37 +260,6 @@ Available launch file options:
 
 
 
-### Example 3: "Industrial robot with integrated sensor"
-
-- Launch file: [rrbot_system_with_sensor.launch.py](ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py)
-- Controllers: [rrbot_with_sensor_controllers.yaml](ros2_control_demo_bringup/config/rrbot_with_sensor_controllers.yaml)
-- URDF: [rrbot_system_with_sensor.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro)
-- ros2_control URDF: [rrbot_system_with_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro)
-
-- Command interfaces:
-  - joint1/position
-  - joint2/position
-- State interfaces:
-  - joint1/position
-  - joint2/position
-  - tcp_fts_sensor/force.x
-  - tcp_fts_sensor/torque.z
-
-Available controllers:
-- `forward_position_controller[forward_command_controller/ForwardCommandController]`
-- `fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`
-- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`
-
-Notes:
-  - Wrench messages are may not be displayed properly in Rviz as NaN values are not handled in Rviz and FTS Broadcaster may send NaN values.
-
-Commanding the robot: see the commands below.
-
-Accessing Wrench data from 2D FTS:
-```
-ros2 topic echo /fts_broadcaster/wrench
-```
-
 
 ### Example 5: "Modular Robots with separate communication to each actuator"
 

example_4/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2_control_demo_example_4)
+
+# 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_with_sensor.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_4_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_4.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_4/README.rst

@@ -0,0 +1,134 @@
+***************************************************
+Example 4: Industrial robot with integrated sensor
+***************************************************
+
+This example shows how a sensor can be integrated in a hardware interface of system-type:
+A 2D Force-Torque Sensor (FTS) is simulated by generating random sensor readings.
+
+1. To check that *RRBot* descriptions are working properly use following launch commands
+
+   .. code-block:: shell
+
+    ros2 launch ros2_control_demo_example_4 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_4 rrbot_system_with_sensor.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
+            tcp_fts_sensor/force.x
+            tcp_fts_sensor/torque.z
+
+   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
+
+    joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
+    fts_broadcaster     [force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active
+    forward_position_controller[forward_command_controller/ForwardCommandController] active
+
+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_4 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
+
+    [RRBotSystemWithSensorHardware]: Got command 0.50000 for joint 0!
+    [RRBotSystemWithSensorHardware]: Got command 0.50000 for joint 1!
+
+6. Access wrench data from 2D FTS via
+
+   .. code-block:: shell
+
+    ros2 topic echo /fts_broadcaster/wrench
+
+   shows the random generated sensor values, republished by *Force Torque Sensor Broadcaster* as
+   ``geometry_msgs/msg/WrenchStamped`` message
+
+   .. code-block:: shell
+
+    header:
+      stamp:
+        sec: 1676444704
+        nanosec: 332221422
+      frame_id: tool_link
+    wrench:
+      force:
+        x: 2.946532964706421
+        y: .nan
+        z: .nan
+      torque:
+        x: .nan
+        y: .nan
+        z: 4.0540995597839355
+
+   .. warning::
+    Wrench messages are not displayed properly in *RViz* as NaN values are not handled in *RViz* and FTS Broadcaster may send NaN values.
+
+
+Files used for this demo
+#########################
+
+- Launch file: `rrbot_system_with_sensor.launch.py <https://github.com/ros-controls/ros2_control_demos/example_4/bringup/launch/rrbot_system_with_sensor.launch.py>`__
+- Controllers yaml: `rrbot_with_sensor_controllers.yaml <https://github.com/ros-controls/ros2_control_demos/example_4/bringup/config/rrbot_with_sensor_controllers.yaml>`__
+- URDF: `rrbot_system_with_sensor.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/example_4/description/urdf/rrbot_system_with_sensor.urdf.xacro>`__
+
+  + ``ros2_control`` URDF tag: `rrbot_system_with_sensor.ros2_control.xacro <https://github.com/ros-controls/ros2_control_demos/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro>`__
+
+- RViz configuration: `rrbot.rviz <https://github.com/ros-controls/ros2_control_demos/example_4/description/rviz/rrbot.rviz>`__
+- Hardware interface plugin: `rrbot_system_with_sensor.cpp <https://github.com/ros-controls/ros2_control_demos/example_4/hardware/rrbot_system_with_sensor.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>`__
+- ``Force Torque Sensor Broadcaster`` (`ros2_controllers repository <https://github.com/ros-controls/ros2_controllers>`__): `doc <https://control.ros.org/master/doc/ros2_controllers/force_torque_sensor_broadcaster/doc/userdoc.html>`__

example_4/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]