Add corrections and extensions for the rrbot (#71)

* Add corrections and extensions for the rrbot

* Correct build and remove demo nodes from readme.

* Apply suggestions from code review

Co-authored-by: Bence Magyar <[email protected]>

* Adjust colors of the robot

* Update ros2_control_demo_robot/package.xml

Co-authored-by: Victor Lopez <[email protected]>

* Update ros2_control_demo_robot/launch/test_rrbot_description.launch.py

Co-authored-by: Bence Magyar <[email protected]>
Co-authored-by: Victor Lopez <[email protected]>

Author: 3 people

README.md

@@ -1,11 +1,11 @@
-# ROS2 Control Demos
+# ros2_control Demos
 
 [![Build Status](https://github.com/ros-controls/ros2_control_demos/workflows/CI/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ACI)
 [![Linters Status](https://github.com/ros-controls/ros2_control_demos/workflows/Linters/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ALinters)
 [![Coverage Status](https://github.com/ros-controls/ros2_control_demos/workflows/Coverage/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ACoverage)
 [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
 
-This repository provides templates for the development of `ros2_control`-enabled robots and a simple simulation of a robot.
+This repository provides templates for the development of `ros2_control`-enabled robots and a simple simulations to demonstrate and prove `ros2_control` concepts.
 
 ## Goals
 
@@ -46,10 +46,15 @@ These are some quick hints, especially for those coming from a ROS1 control back
 * <ros2_control> tags in the URDF must be compatible with the controller's configuration.
 * PLUGINLIB_EXPORT_CLASS macro is required when implementing an interface.
 
-# Test of the Scenario Before the First Release
-* Checkout [ros-controls/ros2_control](https://github.com/ros-controls/ros2_control) to get the core.
-* Checkout [ros-controls/ros2_controllers](https://github.com/ros-controls/ros2_controllers) to get all the controllers.
-* Checkout [ros-controls/ros2_control_demos](https://github.com/ros-controls/ros2_control_demos) to get example hardware and robot launch files.
+# Build from source
+```
+git clone https://github.com/ros-controls/ros2_control
+git clone https://github.com/ros-controls/ros2_controllers
+git clone https://github.com/ros-controls/ros2_control_demos
+```
+
+**NOTE**: `ros2_control` and `ros2_controllers` packages are released for foxy and can be installed using package manager.
+  For daily use it is recommended to use the released version but there may always be some not-yet-released changes that are required to build the demos.
 
 * Install dependencies (maybe you need `sudo`):
   ```
@@ -64,80 +69,125 @@ These are some quick hints, especially for those coming from a ROS1 control back
 * Do not forget to source `setup.bash` from the `install` folder!
 
 
-# Getting Started with ROS2 Control
+# Getting Started with ros2_control
 
 Each of the described example cases from the roadmap has its own launch and URDF file.
 
-## Example 1: "Industrial Robots with only one interface"
-
-1. Start the roslaunch file:
-  ```
-  ros2 launch ros2_control_demo_robot rrbot_system_position_only.launch.py
-  ```
-
-2. Open another terminal and check if `RRBotSystemPositionOnlyHardware` is loaded properly:
-  ```
-  ros2 control list_hardware_interfaces
-  ```
-  You should get something like:
-  ```
-  command interfaces
-        joint1/position [unclaimed]
-        joint2/position [unclaimed]
-  state interfaces
-         joint1/position
-         joint2/position
-  ```
-
-3. Open another terminal and load, configure, and start controllers:
-  ```
-  ros2 control load_start_controller joint_state_controller
-  ros2 control load_configure_controller forward_command_controller_position
-  ```
-  
-  Check if the controller is loaded properly:
-  ```
-  ros2 control list_controllers
-  ```
-  You should get the response:
-  ```
-  joint_state_controller[joint_state_controller/JointStateController] active  
-  forward_command_controller_position[forward_command_controller/ForwardCommandController] inactive
-  ```
-
-4. Starting controller:
-  ```
-  ros2 control switch_controllers --start-controllers forward_command_controller_position 
-  ```
-  
-  Check if controllers are activated:
-  ```
-  ros2 control list_controllers
-  ```
-  You should get `active` in the response:
-  ```
-  joint_state_controller[joint_state_controller/JointStateController] active    
-  forward_command_controller_position[forward_command_controller/ForwardCommandController] active
-  ```
-  
-5. Open another terminal and send a message to the controller:
-  ```
-  ros2 topic pub /forward_command_controller_position/commands std_msgs/msg/Float64MultiArray "data: 
-  - 0.5                                                               
-  - 0.5"
-  ```
-  
-  You should see how the example output changes. Look for the following lines
-  ```
-  [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 0!
-  [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 1!
-  ```
-  
-  If you echo the `/joint_states` or `/dynamic_joint_states` topics you should also get similar values.
-  ```
-  ros2 topic echo /joint_states
-  ros2 topic echo /dynamic_joint_states
-  ```
-
-The other launch-files have corresponding names to their coresponding example.
-The URDF files can be found in the `description` folder.
+## Starting example robots
+
+Each example is started with a single launch file which starts up the robot hardware, loads controller configurations and it also opens `rviz2`. 
+
+The `rviz2` setup can be recreated following these steps:
+
+- The robot models can be visualized using `RobotModel` display using `/robot_description` topic.
+- Or you can simply open the configuration from `rviz` folder in `ros2_control_demo_robot` package manually or directly by executing:
+  ```
+  rviz2 --display-config `ros2 pkg prefix ros2_control_demo_robot`/share/ros2_control_demo_robot/rviz/rrbot.rviz
+  ```
+
+*RRBot*, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm that we will use to demonstrate various features. It essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials.
+The *RRbot* URDF files can be found in the `description` folder of `ros2_control_demo_robot` package.
+
+### Example 1: "Industrial Robots with only one interface"
+
+1. Open another terminal and start the roslaunch file:
+   ```
+   ros2 launch ros2_control_demo_robot rrbot_system_position_only.launch.py
+   ```
+
+2. Open another terminal and check that `RRBotSystemPositionOnlyHardware` loaded properly:
+   ```
+   ros2 control list_hardware_interfaces
+   ```
+   You should get something like:
+   ```
+   command interfaces
+         joint1/position [unclaimed]
+         joint2/position [unclaimed]
+   state interfaces
+          joint1/position
+          joint2/position
+   ```
+
+## Controlles and moving hardware
+To move the robot you should load and start controllers.
+The `JointStateController` is used to publish the joint states to ROS topics.
+Direct joint commands are sent to this robot via the `ForwardCommandController`.
+The sections below describe their usage.
+Check the [Results](##result) section on how to ensure that things went well.
+
+### JointStateController
+
+Open another terminal and load, configure and start `joint_state_controller`:
+```
+ros2 control load_start_controller joint_state_controller
+```
+Check if controller is loaded properly:
+```
+ros2 control list_controllers
+```
+You should get the response:
+```
+joint_state_controller[joint_state_controller/JointStateController] active
+```
+
+Now you should also see the *RRbot* represented correctly in `rviz2`.
+
+
+### Using ForwardCommandController
+
+1. If you want to test hardware with `ForwardCommandController` first load and configure it:
+   ```
+   ros2 control load_configure_controller forward_position_controller
+   ```
+   Check if the controller is loaded properly:
+   ```
+   ros2 control list_controllers
+   ```
+   You should get the response:
+   ```
+   joint_state_controller[joint_state_controller/JointStateController] active
+   forward_position_controller[forward_command_controller/ForwardCommandController] inactive
+   ```
+
+2. Now start the controller:
+   ```
+   ros2 control switch_controllers --start-controllers forward_position_controller
+   ```
+   Check if controllers are activated:
+   ```
+   ros2 control list_controllers
+   ```
+   You should get `active` in the response:
+   ```
+   joint_state_controller[joint_state_controller/JointStateController] active
+   forward_position_controller[forward_command_controller/ForwardCommandController] active
+   ```
+
+**NOTE**: You can do this in only one step by using `load_start_controller` verb instead of `load_configure_controller`.
+
+3. Send command to the controller, either:
+
+   a. Manually using ros2 cli interface:
+   ```
+   ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
+   - 0.5
+   - 0.5"
+   ```
+
+## Result
+
+1. Independently from the controller you should see how the example's output changes.
+  Look for the following lines
+   ```
+   [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 0!
+   [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 1!
+   ```
+
+2. If you echo the `/joint_states` or `/dynamic_joint_states` topics you should also get similar values.
+   ```
+   ros2 topic echo /joint_states
+   ros2 topic echo /dynamic_joint_states
+   ```
+
+3. You should also see the *RRbot* moving in `rviz2`.

ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp

@@ -20,18 +20,15 @@
 #include <string>
 #include <vector>
 
-#include "rclcpp/macros.hpp"
-
 #include "hardware_interface/base_interface.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_status_values.hpp"
+#include "rclcpp/macros.hpp"
 #include "ros2_control_demo_hardware/visibility_control.h"
 
-using hardware_interface::return_type;
-
 namespace ros2_control_demo_hardware
 {
 class RRBotSystemPositionOnlyHardware : public
@@ -41,7 +38,7 @@ class RRBotSystemPositionOnlyHardware : public
   RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware);
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
-  return_type configure(const hardware_interface::HardwareInfo & info) override;
+  hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
   std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
@@ -50,16 +47,16 @@ class RRBotSystemPositionOnlyHardware : public
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
-  return_type start() override;
+  hardware_interface::return_type start() override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
-  return_type stop() override;
+  hardware_interface::return_type stop() override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
-  return_type read() override;
+  hardware_interface::return_type read() override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
-  return_type write() override;
+  hardware_interface::return_type write() override;
 
 private:
   // Parameters for the RRBot simulation