Unverified Commit 046ce0c5 authored by Denis Štogl's avatar Denis Štogl Committed by GitHub
Browse files

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: default avatarBence Magyar <bence.magyar.robotics@gmail.com>

* Adjust colors of the robot

* Update ros2_control_demo_robot/package.xml
Co-authored-by: default avatarVictor Lopez <3469405+v-lopez@users.noreply.github.com>

* Update ros2_control_demo_robot/launch/test_rrbot_description.launch.py
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: default avatarVictor Lopez <3469405+v-lopez@users.noreply.github.com>
parent 620b703e
# 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`.
......@@ -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 @@ 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 @@ 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
......
......@@ -26,11 +26,11 @@
namespace ros2_control_demo_hardware
{
return_type RRBotSystemPositionOnlyHardware::configure(
hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure(
const hardware_interface::HardwareInfo & info)
{
if (configure_default(info) != return_type::OK) {
return return_type::ERROR;
if (configure_default(info) != hardware_interface::return_type::OK) {
return hardware_interface::return_type::ERROR;
}
hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
......@@ -46,7 +46,7 @@ return_type RRBotSystemPositionOnlyHardware::configure(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d command interfaces found. 1 expected.",
joint.name.c_str(), joint.command_interfaces.size());
return return_type::ERROR;
return hardware_interface::return_type::ERROR;
}
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
......@@ -55,7 +55,7 @@ return_type RRBotSystemPositionOnlyHardware::configure(
"Joint '%s' have %s command interfaces found. '%s' expected.",
joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION);
return return_type::ERROR;
return hardware_interface::return_type::ERROR;
}
if (joint.state_interfaces.size() != 1) {
......@@ -63,7 +63,7 @@ return_type RRBotSystemPositionOnlyHardware::configure(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d state interface. 1 expected.",
joint.name.c_str(), joint.state_interfaces.size());
return return_type::ERROR;
return hardware_interface::return_type::ERROR;
}
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
......@@ -72,12 +72,12 @@ return_type RRBotSystemPositionOnlyHardware::configure(
"Joint '%s' have %s state interface. '%s' expected.",
joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION);
return return_type::ERROR;
return hardware_interface::return_type::ERROR;
}
}
status_ = hardware_interface::status::CONFIGURED;
return return_type::OK;
return hardware_interface::return_type::OK;
}
std::vector<hardware_interface::StateInterface>
......@@ -107,7 +107,7 @@ RRBotSystemPositionOnlyHardware::export_command_interfaces()
}
return_type RRBotSystemPositionOnlyHardware::start()
hardware_interface::return_type RRBotSystemPositionOnlyHardware::start()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
......@@ -120,11 +120,13 @@ return_type RRBotSystemPositionOnlyHardware::start()
"%.1f seconds left...", hw_start_sec_ - i);
}
// set some default values
// set some default values when starting the first time
for (uint i = 0; i < hw_states_.size(); i++) {
if (std::isnan(hw_states_[i])) {
hw_states_[i] = 0;
hw_commands_[i] = 0;
} else {
hw_commands_[i] = hw_states_[i];
}
}
......@@ -134,10 +136,10 @@ return_type RRBotSystemPositionOnlyHardware::start()
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System Sucessfully started!");
return return_type::OK;
return hardware_interface::return_type::OK;
}
return_type RRBotSystemPositionOnlyHardware::stop()
hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
......@@ -156,7 +158,7 @@ return_type RRBotSystemPositionOnlyHardware::stop()
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System sucessfully stopped!");
return return_type::OK;
return hardware_interface::return_type::OK;
}
hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
......@@ -167,7 +169,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
for (uint i = 0; i < hw_states_.size(); i++) {
// Simulate RRBot's movement
hw_states_[i] = hw_commands_[i] + (hw_states_[i] - hw_commands_[i]) / hw_slowdown_;
hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Got state %.5f for joint %d!", hw_states_[i], i);
......@@ -176,10 +178,10 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joints sucessfully read!");
return return_type::OK;
return hardware_interface::return_type::OK;
}
hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware::write()
hardware_interface::return_type RRBotSystemPositionOnlyHardware::write()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
......@@ -195,7 +197,7 @@ hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemPositionO
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joints sucessfully written!");
return return_type::OK;
return hardware_interface::return_type::OK;
}
} // namespace ros2_control_demo_hardware
......
......@@ -15,7 +15,7 @@ find_package(ament_cmake REQUIRED)
## INSTALL
install(
DIRECTORY controllers description launch/
DIRECTORY config description launch rviz
DESTINATION share/${PROJECT_NAME}
)
......
......@@ -2,13 +2,16 @@ controller_manager:
ros__parameters:
update_rate: 2 # Hz
forward_command_controller_position:
type: forward_command_controller/ForwardCommandController
joint_state_controller:
type: joint_state_controller/JointStateController
forward_command_controller_position:
forward_position_controller:
type: forward_command_controller/ForwardCommandController
position_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
forward_position_controller:
ros__parameters:
joints:
- joint1
......
......@@ -33,6 +33,10 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="yellow">
<color rgba="1.0 1.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
......
......@@ -4,10 +4,9 @@
<xacro:macro name="rrbot" params="parent prefix *origin">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="mass" value="1" /> <!-- arbitrary value for mass -->
<xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="height1" value="2" /> <!-- Link 1 -->
<xacro:property name="height1" value="0.2" /> <!-- Link 1 -->
<xacro:property name="height2" value="1" /> <!-- Link 2 -->
<xacro:property name="height3" value="1" /> <!-- Link 3 -->
<xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
......@@ -68,7 +67,7 @@
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
<material name="black"/>
<material name="yellow"/>
</visual>
<inertial>
......
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rrbot_system_position_only" params="name prefix">
<xacro:macro name="rrbot_system_position_only" params="name prefix slowdown:=2.0">
<ros2_control name="${name}" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_hw_start_duration_sec">2.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">2.0</param>
<param name="example_param_hw_slowdown">${slowdown}</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
......
......@@ -6,21 +6,20 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
<!-- Enable setting arguments from the launch file -->
<xacro:arg name="slowdown" default="2.0" />
<!-- Import RRBot macro -->
<xacro:include filename="rrbot/rrbot.urdf.xacro" />
<!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.urdf.xacro" /> -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.urdf.xacro" />
<!-- Import all Gazebo-customization elements, including Gazebo colors -->
<xacro:include filename="rrbot/rrbot.gazebo.xacro" />
<!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.gazebo.xacro" /> -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.gazebo.xacro" />
<!-- Import Rviz colors -->
<xacro:include filename="rrbot/rrbot.materials.xacro" />
<!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.materials.xacro" /> -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.materials.xacro" />
<!-- Import RRBot ros2_control description -->
<xacro:include filename="rrbot/rrbot_system_position_only.ros2_control.xacro" />
<!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot_system_position_only.ros2_control.xacro" /> -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot_system_position_only.ros2_control.xacro" />
<!-- Used for fixing robot -->
<link name="world"/>
......@@ -34,6 +33,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
<xacro:rrbot_gazebo prefix="" />
<xacro:rrbot_system_position_only name="RRBotSystemPositionOnly" prefix="" />
<xacro:rrbot_system_position_only name="RRBotSystemPositionOnly" prefix="" slowdown="$(arg slowdown)" />
</robot>
# Copyright 2020 ROS2-Control Development Team (2020)
# 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.
......@@ -29,24 +29,46 @@ def generate_launch_description():
get_package_share_directory('ros2_control_demo_robot'),
'description',
'rrbot_system_position_only.urdf.xacro')
robot_description_config = xacro.process_file(robot_description_path)
robot_description_config = xacro.process_file(robot_description_path,
mappings={'slowdown': '3.0'})
robot_description = {'robot_description': robot_description_config.toxml()}
rrbot_forward_controller = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'controllers',
'rrbot_forward_controller_position.yaml'
'config',
'rrbot_controllers.yaml'
)
rviz_config_file = os.path.join(
get_package_share_directory('ros2_control_demo_robot'),
'rviz',
'rrbot.rviz'
)
return LaunchDescription([
Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[robot_description, rrbot_forward_controller],
output={
control_node = Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[robot_description, rrbot_forward_controller],
output={
'stdout': 'screen',
'stderr': 'screen',
},
)
},
)
robot_state_publisher_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],
)
return LaunchDescription([
control_node,
robot_state_publisher_node,
rviz_node,
])
# 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