Commit e49bec2e authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Add directory

parent 332a6190
// 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__SYSTEM_BOLT_MULTI_INTERFACE_HPP_
#define ROS2_CONTROL_DEMO_HARDWARE__SYSTEM_BOLT_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 SystemBoltMultiInterfaceHardware
: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(SystemBoltMultiInterfaceHardware);
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 prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_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 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__SYSTEM_BOLT_MULTI_INTERFACE_HPP_
// 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_hardware_interface_bolt/system_bolt_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 SystemBoltMultiInterfaceHardware::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)
{
// SystemBoltMultiInterface has exactly 3 state interfaces
// and 3 command interfaces on each joint
if (joint.command_interfaces.size() != 3)
{
RCLCPP_FATAL(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"),
"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("SystemBoltMultiInterfaceHardware"),
"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("SystemBoltMultiInterfaceHardware"),
"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("SystemBoltMultiInterfaceHardware"),
"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>
SystemBoltMultiInterfaceHardware::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>
SystemBoltMultiInterfaceHardware::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 SystemBoltMultiInterfaceHardware::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 SystemBoltMultiInterfaceHardware::start()
{
RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"), "Starting... please wait...");
for (int i = 0; i <= hw_start_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"), "%.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("SystemBoltMultiInterfaceHardware"), "System successfully started! %u",
control_lvl_[0]);
return return_type::OK;
}
return_type SystemBoltMultiInterfaceHardware::stop()
{
RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"), "Stopping... please wait...");
for (int i = 0; i <= hw_stop_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"), "%.1f seconds left...",
hw_stop_sec_ - i);
}
status_ = hardware_interface::status::STOPPED;
RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"), "System successfully stopped!");
return return_type::OK;
}
return_type SystemBoltMultiInterfaceHardware::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("SystemBoltMultiInterfaceHardware"),
"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("SystemBoltMultiInterfaceHardware"),
"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 SystemBoltMultiInterfaceHardware::write()
{
/*RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"),
"Writing...");*/
for (std::size_t i = 0; i < hw_commands_positions_.size(); i++)
{
// Simulate sending commands to the hardware
RCLCPP_INFO(
rclcpp::get_logger("SystemBoltMultiInterfaceHardware"),
"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::SystemBoltMultiInterfaceHardware,
hardware_interface::SystemInterface)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment