Unverified Commit 3dfe077e authored by Denis Štogl's avatar Denis Štogl Committed by GitHub
Browse files

Add first example robot (rrbot as system) (#37)



* Implementation of RRBot System Headless Hardware with only position joints.

* rrbot system position only after first testing.

* Added description of rrbot and example ros2_control_manager.

* ROS2 Managers are moved to controller_interface package

* Update README.md

* Change package names to represent the actual state.
* Describe connection to the example URDFs

* Add controlle to example 1 and adapt the hardware to provide more sensible example.

* Added controller to install path

* Change parameter names because deprecated

* Reorganize controller's yaml to have correct parameters' namespace. Comment out the name of the node from the roslaunch file to avoid issue with LifecycleNode-Names.
Add better robot's movement simulation.

* Corrected licence header and add explicit type instead of auto

* Update README.md

* Update parameter and launch

* Update README.md

* Reorganize to new RM structure

* Update README.md

* Updated to new status

* Adapt brances and additional output

* Add parsing robot_description via xacro. (#45)

* Add parsing robot_description via xacro.
Co-authored-by: default avatarDenis Štogl <denis@stogl.de>

* Remove URDF file

* Update description

* Update linters.yaml

* Update ci.yml

* Add dependencies

* Update ci.yml

* Update CMakeLists.txt

* Update package.xml

* Delete code_coverage.yml

* Update ci.yml

* Update ci.yml

* Update linters.yaml

* Update linters.yaml

* Update ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>

* Update ros2_control_demo_hardware/src/rrbot_system_position_only.cpp
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>

* Address review comments.
Co-authored-by: default avatarlivanov93 <lovro.ivanov@gmail.com>
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>
parent 7b4968d2
......@@ -9,14 +9,49 @@ on:
- cron: '28 6 * * *'
jobs:
industrial_ci:
ci_binary:
name: Foxy binary job
runs-on: ubuntu-latest
strategy:
matrix:
env:
- {ROS_DISTRO: foxy, ROS_REPO: testing}
- {ROS_DISTRO: foxy, ROS_REPO: main}
runs-on: ubuntu-latest
- {ROS_DISTRO: foxy, ROS_REPO: testing}
env:
UPSTREAM_WORKSPACE: ros2_control_demos/ros2_control_demos.repos
steps:
- run: sudo apt-get update -qq && sudo apt-get upgrade
- uses: actions/checkout@v1
- uses: 'ros-industrial/industrial_ci@master'
env: ${{matrix.env}}
ci_source:
name: Foxy source job
runs-on: ubuntu-20.04
strategy:
fail-fast: false
steps:
- uses: ros-tooling/setup-ros@0.0.26
with:
required-ros-distributions: foxy
- uses: ros-tooling/action-ros-ci@0.1.0
with:
target-ros2-distro: foxy
# build all packages listed in the meta package
package-name:
ros2_control_demo_hardware
ros2_control_demo_robot
ros2_control_demos
vcs-repo-file-url: |
https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control_demos/ros2_control_demos.repos
colcon-mixin-name: coverage-gcc
colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
- uses: codecov/codecov-action@v1.0.14
with:
file: ros_ws/lcov/total_coverage.info
flags: unittests
name: codecov-umbrella
- uses: actions/upload-artifact@v1
with:
name: colcon-logs-${{ matrix.os }}
path: ros_ws/log
name: Coverage
on:
pull_request:
push:
branches:
- master
schedule:
# Run every morning to detect flakiness and broken dependencies
- cron: '17 8 * * *'
jobs:
build_and_test:
runs-on: ubuntu-18.04
strategy:
fail-fast: false
steps:
- uses: ros-tooling/setup-ros@0.0.13
- uses: ros-tooling/action-ros-ci@0.0.13
with:
package-name: ros2_control_demo_communication_gazebo ros2_control_demo_communication_headless ros2_control_demo_hardware ros2_control_demo_robot
colcon-mixin-name: coverage-gcc
colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
- uses: codecov/codecov-action@v1
with:
token: ${{ secrets.CODECOV_TOKEN }}
file: ros_ws/lcov/total_coverage.info
flags: unittests
name: codecov-umbrella
yml: ./codecov.yml
fail_ci_if_error: true
# - uses: actions/upload-artifact@master
# with:
# name: colcon-logs-${{ matrix.os }}
# path: ros_ws/log
name: Linters
on:
pull_request:
push:
branches:
- master
jobs:
ament_copyright:
......@@ -11,22 +8,28 @@ jobs:
runs-on: ubuntu-18.04
steps:
- uses: actions/checkout@v1
- uses: ros-tooling/setup-ros@0.0.13
- uses: ros-tooling/action-ros-lint@0.0.5
- uses: ros-tooling/setup-ros@0.0.26
- uses: ros-tooling/action-ros-lint@0.0.6
with:
linter: copyright
package-name: ros2_control_demo_communication_gazebo ros2_control_demo_communication_headless ros2_control_demo_hardware ros2_control_demo_robot
package-name:
ros2_control_demo_hardware
ros2_control_demo_robot
ros2_control_demos
ament_xmllint:
name: ament_xmllint
runs-on: ubuntu-18.04
steps:
- uses: actions/checkout@v1
- uses: ros-tooling/setup-ros@0.0.13
- uses: ros-tooling/action-ros-lint@0.0.5
- uses: ros-tooling/setup-ros@0.0.26
- uses: ros-tooling/action-ros-lint@0.0.6
with:
linter: xmllint
package-name: ros2_control_demo_communication_gazebo ros2_control_demo_communication_headless ros2_control_demo_hardware ros2_control_demo_robot
package-name:
ros2_control_demo_hardware
ros2_control_demo_robot
ros2_control_demos
ament_lint_cpp:
name: ament_${{ matrix.linter }}
......@@ -37,8 +40,11 @@ jobs:
linter: [cppcheck, cpplint, uncrustify]
steps:
- uses: actions/checkout@v1
- uses: ros-tooling/setup-ros@0.0.13
- uses: ros-tooling/action-ros-lint@0.0.5
- uses: ros-tooling/setup-ros@0.0.26
- uses: ros-tooling/action-ros-lint@0.0.6
with:
linter: ${{ matrix.linter }}
package-name: ros2_control_demo_communication_gazebo ros2_control_demo_communication_headless ros2_control_demo_hardware ros2_control_demo_robot
package-name:
ros2_control_demo_hardware
ros2_control_demo_robot
ros2_control_demos
......@@ -11,7 +11,127 @@ This repository provides templates for the development of `ros2_control`-enabled
## Goals
The repository has two goals:
The repository has three goals:
1. Implements the example configuration described in the `ros-controls/roadmap` repository file [components_architecture_and_urdf_examples](https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md).
2. It provides templates for faster start of implementing own hardware and controllers;
3. The repository is a validation environment for `ros2_control` concepts, which can only be tested during run-time (e.g., execution of controllers by the controller manager, communication between robot hardware and controllers).
1. It provides templates for faster start of implementing own hardware and controllers;
2. The repository is a validation environment for `ros2_control` concepts, which can only be tested during run-time (e.g., execution of controllers by the controller manager, communication between robot hardware and controllers).
## Description
The repository is inspired by [ros_control_boilerplate](https://github.com/PickNikRobotics/ros_control_boilerplate) repository from Dave Coleman.
The simulation has three parts/packages:
1. The first package, `ros2_control_demo_hardware`, implements the hardware interfaces described in the roadmap.
This implemented examples simulate *RRbot* internally to provide sufficient test and demonstration data, but to reduce amount of package dependencies.
This package does not have any dependencies except on the `ros2` core packages and can, therefore, be used on SoC-hardware of headless systems.
2. The second package, `ros2_control_demo_hardware_gazebo`, uses a gazebo simulator to simulate the *RRBot* and its physics.
This package is useful to test the connection of `ros2_control` to the gazebo simulator and to detect any missing plugins.
3. The third package `ros2_control_demo_robot` holds examples for *RRbot* URDF-description, launch files and controllers.
The intention of those files is to simplify start with `ros2_control` and to enable faster integration of new robots and controllers.
This repository demonstrates the following `ros2_control` concepts:
* Creating of `*HardwareInterface` for a System, Sensor, and Actuator.
* Creating a robot description in the form of URDF files
* Loading configuration and starting robot using launch files
* Control of two joints of *RRBot*
* Using simulated robots and starting `ros_control` with gazebo simulator
* Implementing of controller switching strategy for a robot
* Using joint limits and transmission concepts in `ros2_control`
* TBD...
# 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.
* Install dependencies (maybe you need `sudo`):
```
apt install ros-foxy-realtime-tools ros-foxy-xacro ros-foxy-angles
```
* Build everything, e.g. with:
```
colcon build --symlink-install
```
* Do not forget to source `setup.bash` from the `install` folder!
# 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 controllers:
```
ros2 control load joint_state_controller
ros2 control load forward_command_controller_position
```
Check if controller is loaded properly:
```
ros2 control list
```
You should get the response:
```
joint_state_controller[joint_state_controller/JointStateController] inactive
forward_command_controller_position[forward_command_controller/ForwardCommandController] inactive
```
4. Starting controller:
```
ros2 control switch --start-controllers joint_state_controller forward_command_controller_position
```
Check if controllers are activated:
```
ros2 control list
```
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.
......@@ -12,7 +12,35 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
## COMPILE
add_library(
${PROJECT_NAME}
SHARED
src/rrbot_system_position_only.cpp
)
target_include_directories(
${PROJECT_NAME}
PRIVATE
include
)
ament_target_dependencies(
${PROJECT_NAME}
hardware_interface
pluginlib
rclcpp
)
pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml)
# INSTALL
install(
TARGETS ${PROJECT_NAME}
DESTINATION lib
)
install(
DIRECTORY include/
DESTINATION include
......@@ -22,10 +50,16 @@ if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
endif()
## EXPORTS
ament_export_include_directories(
include
)
ament_export_libraries(
${PROJECT_NAME}
)
ament_export_dependencies(
hardware_interface
pluginlib
rclcpp
)
ament_package()
// Copyright 2020 ros2_control Development Team
//
// 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
// limitations under the License.
#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
#include <memory>
#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 "ros2_control_demo_hardware/visibility_control.h"
using hardware_interface::return_type;
namespace ros2_control_demo_hardware
{
class RRBotSystemPositionOnlyHardware : public
hardware_interface::BaseInterface<hardware_interface::SystemInterface>
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware);
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type configure(const hardware_interface::HardwareInfo & info) override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type start() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type stop() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type read() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type write() override;
private:
// Parameters for the RRBot simulation
double hw_start_sec_;
double hw_stop_sec_;
double hw_slowdown_;
// Store the command for the simulated robot
std::vector<double> hw_commands_;
std::vector<double> hw_states_;
};
} // namespace ros2_control_demo_hardware
#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
......@@ -11,6 +11,10 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<test_depend>ament_cmake_gtest</test_depend>
<export>
......
<library path="ros2_control_demo_hardware">
<class name="ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"
type="ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware"
base_class_type="hardware_interface::SystemInterface">
<description>
The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
</description>
</class>
</library>
// Copyright 2020 ros2_control Development Team
//
// 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
// limitations under the License.
#include "ros2_control_demo_hardware/rrbot_system_position_only.hpp"
#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
namespace ros2_control_demo_hardware
{
return_type RRBotSystemPositionOnlyHardware::configure(
const hardware_interface::HardwareInfo & info)
{
if (configure_default(info) != return_type::OK) {
return return_type::ERROR;
}
hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
// RRBotSystemPositionOnly has exactly one state and command interface on each joint
if (joint.command_interfaces.size() != 1) {
RCLCPP_FATAL(
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;
}
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"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;
}
if (joint.state_interfaces.size() != 1) {
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d state interface. 1 expected.",
joint.name.c_str(), joint.state_interfaces.size());
return return_type::ERROR;
}
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"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;
}
}
status_ = hardware_interface::status::CONFIGURED;
return return_type::OK;
}
std::vector<hardware_interface::StateInterface>
RRBotSystemPositionOnlyHardware::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for (uint i = 0; i < info_.joints.size(); i++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface>
RRBotSystemPositionOnlyHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (uint i = 0; i < info_.joints.size(); i++) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
}
return command_interfaces;
}
return_type RRBotSystemPositionOnlyHardware::start()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Starting ...please wait...");
for (int i = 0; i <= hw_start_sec_; i++) {
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"%.1f seconds left...", hw_start_sec_ - i);
}
// set some default values
for (uint i = 0; i < hw_states_.size(); i++) {
if (std::isnan(hw_states_[i])) {
hw_states_[i] = 0;
hw_commands_[i] = 0;
}
}
status_ = hardware_interface::status::STARTED;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System Sucessfully started!");
return return_type::OK;
}
return_type RRBotSystemPositionOnlyHardware::stop()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Stopping ...please wait...");
for (int i = 0; i <= hw_stop_sec_; i++) {
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"%.1f seconds left...", hw_stop_sec_ - i);
}
status_ = hardware_interface::status::STOPPED;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System sucessfully stopped!");
return return_type::OK;
}
hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Reading...");
for (uint i = 0; i < hw_states_.size(); i++) {
// Simulate RR