Unverified Commit f5d821c2 authored by mahaarbo's avatar mahaarbo Committed by GitHub
Browse files

[Example 2]: Multiple command interfaces using prepare - perform switch (#83)



* prepare - perform switch

* Update ros2_control_demo_robot/config/rrbot_multi_interface_forward_controllers.yaml
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>

* Add demo doc for multi interface

* Trying to appease linter

* Accidental double ifndef, minor style fix

* Apply format

* uint -> size_t

* std::equal -> std::all_of
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: default avatarDenis Štogl <denis@stogl.de>
parent 2b2d9be9
......@@ -4,3 +4,97 @@ Demos
-----
This repository provides templates for the development of ros2_control-enabled robots and a simple simulations to demonstrate and prove ros2_control concepts.
Mode switching demo
^^^^^^^^^^^^^^^^^^^
Start up the multi interface rrbot system:
.. code-block:: bash
ros2 launch ros2_control_demo_robot rrbot_system_multi_interface.launch.py
List the available interfaces
.. code-block:: bash
$ ros2 control list_hardware_interfaces
command interfaces
joint1/acceleration [unclaimed]
joint1/position [unclaimed]
joint1/velocity [unclaimed]
joint2/acceleration [unclaimed]
joint2/position [unclaimed]
joint2/velocity [unclaimed]
state interfaces
joint1/acceleration
joint1/position
joint1/velocity
joint2/acceleration
joint2/position
joint2/velocity
Load and configure all controllers
.. code-block:: bash
ros2 control load_controller forward_command_controller_position --state configure
ros2 control load_controller forward_command_controller_velocity --state configure
ros2 control load_controller forward_command_controller_acceleration --state configure
ros2 control load_controller forward_command_controller_illegal1 --state configure
ros2 control load_controller forward_command_controller_illegal2 --state configure
ros2 control load_controller joint_state_controller --state configure
Start the position controller
.. code-block:: bash
ros2 control set_controller_state forward_command_controller_position start
Check the hardware interfaces, position interfaces should be claimed now
.. code-block:: bash
$ ros2 control list_hardware_interfaces
command interfaces
joint1/acceleration [unclaimed]
joint1/position [claimed]
joint1/velocity [unclaimed]
joint2/acceleration [unclaimed]
joint2/position [claimed]
joint2/velocity [unclaimed]
state interfaces
joint1/acceleration
joint1/position
joint1/velocity
joint2/acceleration
joint2/position
joint2/velocity
Let's switch controllers now to velocity
.. code-block:: bash
ros2 control switch_controllers --stop-controllers forward_command_controller_position --start-controllers forward_command_controller_velocity
List hardware interfaces again to see that indeed position interfaces have been unclaimed while velocity is claimed now
.. code-block:: bash
$ ros2 control list_hardware_interfaces
command interfaces
joint1/acceleration [unclaimed]
joint1/position [unclaimed]
joint1/velocity [claimed]
joint2/acceleration [unclaimed]
joint2/position [unclaimed]
joint2/velocity [claimed]
state interfaces
joint1/acceleration
joint1/position
joint1/velocity
joint2/acceleration
joint2/position
joint2/velocity
......@@ -21,6 +21,7 @@ add_library(
${PROJECT_NAME}
SHARED
src/rrbot_system_position_only.cpp
src/rrbot_system_multi_interface.cpp
)
target_include_directories(
${PROJECT_NAME}
......
// Copyright 2021 Department of Engineering Cybernetics, NTNU
//
// 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_MULTI_INTERFACE_HPP_
#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_MULTI_INTERFACE_HPP_
#include <cstdint>
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/macros.hpp"
#include "hardware_interface/base_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.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 RRBotSystemMultiInterfaceHardware
: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemMultiInterfaceHardware);
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
virtual return_type prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces);
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 commands for the simulated robot
std::vector<double> hw_commands_positions_;
std::vector<double> hw_commands_velocities_;
std::vector<double> hw_commands_accelerations_;
std::vector<double> hw_positions_;
std::vector<double> hw_velocities_;
std::vector<double> hw_accelerations_;
// Enum defining at which control level we are
// Dumb way of maintaining the command_interface type per joint.
enum class integration_lvl_t : std::uint8_t
{
UNDEFINED = 0,
POSITION = 1,
VELOCITY = 2,
ACCELERATION = 3
};
std::vector<integration_lvl_t> control_lvl_;
};
} // namespace ros2_control_demo_hardware
#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_MULTI_INTERFACE_HPP_
......@@ -6,4 +6,11 @@
The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
</description>
</class>
<class name="ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"
type="ros2_control_demo_hardware::RRBotSystemMultiInterfaceHardware"
base_class_type="hardware_interface::SystemInterface">
<description>
The ROS2 Control minimal robot protocol for a system with multiple command and state interfaces.
</description>
</class>
</library>
// Copyright 2021 Department of Engineering Cybernetics, NTNU
//
// 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_multi_interface.hpp"
#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
namespace ros2_control_demo_hardware
{
return_type RRBotSystemMultiInterfaceHardware::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_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_velocities_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_accelerations_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_velocities_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_accelerations_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
control_lvl_.resize(info_.joints.size(), integration_lvl_t::POSITION);
for (const hardware_interface::ComponentInfo & joint : info_.joints)
{
// RRBotSystemMultiInterface has exactly 3 state interfaces
// and 3 command interfaces on each joint
if (joint.command_interfaces.size() != 3)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Joint '%s' has %d command interfaces. 3 expected.", joint.name.c_str());
return return_type::ERROR;
}
if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION ||
joint.command_interfaces[0].name == hardware_interface::HW_IF_VELOCITY ||
joint.command_interfaces[0].name == hardware_interface::HW_IF_ACCELERATION))
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Joint '%s' has %s command interface. Expected %s, %s, or %s.", joint.name.c_str(),
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION);
return return_type::ERROR;
}
if (joint.state_interfaces.size() != 3)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Joint '%s'has %d state interfaces. 3 expected.", joint.name.c_str());
return return_type::ERROR;
}
if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION ||
joint.state_interfaces[0].name == hardware_interface::HW_IF_VELOCITY ||
joint.state_interfaces[0].name == hardware_interface::HW_IF_ACCELERATION))
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Joint '%s' has %s state interface. Expected %s, %s, or %s.", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION);
return return_type::ERROR;
}
}
status_ = hardware_interface::status::CONFIGURED;
return return_type::OK;
}
std::vector<hardware_interface::StateInterface>
RRBotSystemMultiInterfaceHardware::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for (std::size_t i = 0; i < info_.joints.size(); i++)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &hw_accelerations_[i]));
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface>
RRBotSystemMultiInterfaceHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (std::size_t 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_positions_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION,
&hw_commands_accelerations_[i]));
}
return command_interfaces;
}
return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces)
{
// Prepare for new command modes
std::vector<integration_lvl_t> new_modes = {};
for (std::string key : start_interfaces)
{
for (std::size_t i = 0; i < info_.joints.size(); i++)
{
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION)
{
new_modes.push_back(integration_lvl_t::POSITION);
}
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY)
{
new_modes.push_back(integration_lvl_t::VELOCITY);
}
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION)
{
new_modes.push_back(integration_lvl_t::ACCELERATION);
}
}
}
// Example criteria: All joints must be given new command mode at the same time
if (new_modes.size() != info_.joints.size())
{
return return_type::ERROR;
}
// Example criteria: All joints must have the same command mode
if (!std::all_of(new_modes.begin() + 1, new_modes.end(), [&](integration_lvl_t mode) {
return mode == new_modes[0];
}))
{
return return_type::ERROR;
}
// Stop motion on all relevant joints that are stopping
for (std::string key : stop_interfaces)
{
for (std::size_t i = 0; i < info_.joints.size(); i++)
{
if (key.find(info_.joints[i].name) != std::string::npos)
{
hw_commands_velocities_[i] = 0;
hw_commands_accelerations_[i] = 0;
control_lvl_[i] = integration_lvl_t::UNDEFINED; // Revert to undefined
}
}
}
// Set the new command modes
for (std::size_t i = 0; i < info_.joints.size(); i++)
{
if (control_lvl_[i] != integration_lvl_t::UNDEFINED)
{
// Something else is using the joint! Abort!
return return_type::ERROR;
}
control_lvl_[i] = new_modes[i];
}
return return_type::OK;
}
return_type RRBotSystemMultiInterfaceHardware::start()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Starting... please wait...");
for (int i = 0; i <= hw_start_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "%.1f seconds left...",
hw_start_sec_ - i);
}
// Set some default values
for (std::size_t i = 0; i < hw_positions_.size(); i++)
{
if (std::isnan(hw_positions_[i]))
{
hw_positions_[i] = 0;
}
if (std::isnan(hw_velocities_[i]))
{
hw_velocities_[i] = 0;
}
if (std::isnan(hw_accelerations_[i]))
{
hw_accelerations_[i] = 0;
}
if (std::isnan(hw_commands_positions_[i]))
{
hw_commands_positions_[i] = 0;
}
if (std::isnan(hw_commands_velocities_[i]))
{
hw_commands_velocities_[i] = 0;
}
if (std::isnan(hw_commands_accelerations_[i]))
{
hw_commands_accelerations_[i] = 0;
}
control_lvl_[i] = integration_lvl_t::UNDEFINED;
}
status_ = hardware_interface::status::STARTED;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully started! %u",
control_lvl_[0]);
return return_type::OK;
}
return_type RRBotSystemMultiInterfaceHardware::stop()
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Stopping... please wait...");
for (int i = 0; i <= hw_stop_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "%.1f seconds left...",
hw_stop_sec_ - i);
}
status_ = hardware_interface::status::STOPPED;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully stopped!");
return return_type::OK;
}
return_type RRBotSystemMultiInterfaceHardware::read()
{
for (std::size_t i = 0; i < hw_positions_.size(); i++)
{
switch (control_lvl_[i])
{
case integration_lvl_t::UNDEFINED:
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Nothing is using the hardware interface!");
return return_type::OK;
break;
case integration_lvl_t::POSITION:
hw_accelerations_[i] = 0;
hw_velocities_[i] = 0;
hw_positions_[i] = hw_commands_positions_[i];
break;
case integration_lvl_t::VELOCITY:
hw_accelerations_[i] = 0;
hw_velocities_[i] = hw_commands_velocities_[i];
break;
case integration_lvl_t::ACCELERATION:
hw_accelerations_[i] = hw_commands_accelerations_[i];
break;
}
// Using the hw_slowdown_ parameter as a timestep
hw_velocities_[i] += hw_slowdown_ * hw_accelerations_[i];
hw_positions_[i] += hw_slowdown_ * hw_velocities_[i];
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Got pos: %.5f, vel: %.5f, acc: %.5f for joint %d!", hw_positions_[i], hw_velocities_[i],
hw_accelerations_[i], i);
}
return return_type::OK;
}
return_type RRBotSystemMultiInterfaceHardware::write()
{
/*RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Writing...");*/
for (std::size_t i = 0; i < hw_commands_positions_.size(); i++)
{
// Simulate sending commands to the hardware
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"),
"Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %d, control_lvl: %d",
hw_commands_positions_[i], hw_commands_velocities_[i], hw_commands_accelerations_[i], i,
control_lvl_[i]);
}
return return_type::OK;
}
} // namespace ros2_control_demo_hardware
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
ros2_control_demo_hardware::RRBotSystemMultiInterfaceHardware,
hardware_interface::SystemInterface)
controller_manager:
ros__parameters:
update_rate: 2 # Hz
forward_command_controller_position:
type: forward_command_controller/ForwardCommandController
forward_command_controller_velocity:
type: forward_command_controller/ForwardCommandController
forward_command_controller_acceleration:
type: forward_command_controller/ForwardCommandController
forward_command_controller_illegal1:
type: forward_command_controller/ForwardCommandController
forward_command_controller_illegal2:
type: forward_command_controller/ForwardCommandController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
forward_command_controller_position:
ros__parameters:
joints:
- joint1
- joint2
interface_name: position
forward_command_controller_velocity:
ros__parameters:
joints:
- joint1
- joint2
interface_name: velocity
forward_command_controller_acceleration:
ros__parameters:
joints:
- joint1
- joint2
interface_name: acceleration