Skip to content

Refactor prepare_switch method #1417

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Jun 23, 2025
Merged

Refactor prepare_switch method #1417

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
30fc66e
Refactor prepare_switch method
urfeex Jun 17, 2025
b461692
Use const keyword for interface lists
urfeex Jun 23, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,11 @@
#define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_

// System
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include <limits>

// ros2_control hardware_interface
#include "hardware_interface/hardware_info.hpp"
Expand Down Expand Up @@ -114,6 +115,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(URPositionHardwareInterface);
URPositionHardwareInterface();
virtual ~URPositionHardwareInterface();

hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) final;
Expand Down Expand Up @@ -316,6 +318,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
const std::string FORCE_MODE_GPIO = "force_mode";
const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode";
const std::string TOOL_CONTACT_GPIO = "tool_contact";

std::unordered_map<std::string, std::unordered_map<std::string, bool>> mode_compatibility_;
};
} // namespace ur_robot_driver

Expand Down
278 changes: 92 additions & 186 deletions ur_robot_driver/src/hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,45 @@ namespace rtde = urcl::rtde_interface;
namespace ur_robot_driver
{

URPositionHardwareInterface::URPositionHardwareInterface()
{
mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[hardware_interface::HW_IF_POSITION][FORCE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true;

mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true;

mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true;
mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;

mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true;
mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true;

mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;

mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false;
mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false;
}

URPositionHardwareInterface::~URPositionHardwareInterface()
{
}
Expand Down Expand Up @@ -1099,66 +1138,41 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
}
}

auto is_mode_compatible = [this](const std::string& mode, const std::vector<std::string>& other_modes) {
for (auto& other : other_modes) {
if (mode == other)
continue;

if (mode_compatibility_[mode][other] == false) {
RCLCPP_ERROR(get_logger(), "Starting %s together with %s is not allowed. ", mode.c_str(), other.c_str());
return false;
}
}
return true;
};

// Starting interfaces
// If a joint has been reserved already, raise an error.
// Modes that are not directly mapped to a single joint such as force_mode reserve all joints.
for (const auto& key : start_interfaces) {
for (auto i = 0u; i < info_.joints.size(); i++) {
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
})) {
RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is another control mode already "
"requested.");
return hardware_interface::return_type::ERROR;
}
start_modes_[i].push_back(hardware_interface::HW_IF_POSITION);
} else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
})) {
RCLCPP_ERROR(get_logger(), "Attempting to start velocity control while there is another control mode already "
"requested.");
return hardware_interface::return_type::ERROR;
}
start_modes_[i].push_back(hardware_interface::HW_IF_VELOCITY);
} else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
})) {
RCLCPP_ERROR(get_logger(), "Attempting to start force_mode control while there is either position or "
"velocity mode already requested by another controller.");
return hardware_interface::return_type::ERROR;
}
start_modes_[i].push_back(FORCE_MODE_GPIO);
} else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
})) {
RCLCPP_ERROR(get_logger(), "Attempting to start trajectory passthrough control while there is either "
"position or velocity mode already requested by another controller.");
return hardware_interface::return_type::ERROR;
}
start_modes_[i].push_back(PASSTHROUGH_GPIO);
} else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY ||
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO;
})) {
return hardware_interface::return_type::ERROR;
}
start_modes_[i].push_back(FREEDRIVE_MODE_GPIO);
} else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") {
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
})) {
RCLCPP_ERROR(get_logger(), "Attempting to start tool contact controller while either the force mode or "
"freedrive controller is running.");
return hardware_interface::return_type::ERROR;
std::vector<std::pair<std::string, std::string>> start_modes_to_check{
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION },
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY },
{ tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO },
{ tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO },
{ tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO },
{ tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO }
};

for (auto& item : start_modes_to_check) {
if (key == item.first) {
if (!is_mode_compatible(item.second, start_modes_[i])) {
return hardware_interface::return_type::ERROR;
}
start_modes_[i].push_back(item.second);
continue;
}
start_modes_[i].push_back(TOOL_CONTACT_GPIO);
}
}
}
Expand All @@ -1173,143 +1187,35 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
// add stop interface per joint in tmp var for later check
for (const auto& key : stop_interfaces) {
for (auto i = 0u; i < info_.joints.size(); i++) {
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
stop_modes_[i].push_back(StoppingInterface::STOP_POSITION);
control_modes[i].erase(
std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[](const std::string& item) { return item == hardware_interface::HW_IF_POSITION; }),
control_modes[i].end());
}
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
stop_modes_[i].push_back(StoppingInterface::STOP_VELOCITY);
control_modes[i].erase(
std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[](const std::string& item) { return item == hardware_interface::HW_IF_VELOCITY; }),
control_modes[i].end());
}
if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") {
stop_modes_[i].push_back(StoppingInterface::STOP_FORCE_MODE);
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[&](const std::string& item) { return item == FORCE_MODE_GPIO; }),
control_modes[i].end());
}
if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
stop_modes_[i].push_back(StoppingInterface::STOP_PASSTHROUGH);
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[&](const std::string& item) { return item == PASSTHROUGH_GPIO; }),
control_modes[i].end());
}
if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
stop_modes_[i].push_back(StoppingInterface::STOP_FREEDRIVE);
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }),
control_modes[i].end());
}
if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") {
stop_modes_[i].push_back(StoppingInterface::STOP_TOOL_CONTACT);
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[&](const std::string& item) { return item == TOOL_CONTACT_GPIO; }),
control_modes[i].end());
std::vector<std::tuple<std::string, std::string, StoppingInterface>> stop_modes_to_check{
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION,
StoppingInterface::STOP_POSITION },
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY,
StoppingInterface::STOP_VELOCITY },
{ tf_prefix + FORCE_MODE_GPIO + "/disable_cmd", FORCE_MODE_GPIO, StoppingInterface::STOP_FORCE_MODE },
{ tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO,
StoppingInterface::STOP_PASSTHROUGH },
{ tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO, StoppingInterface::STOP_FREEDRIVE },
{ tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO,
StoppingInterface::STOP_TOOL_CONTACT }
};
for (auto& item : stop_modes_to_check) {
if (key == std::get<0>(item)) {
stop_modes_[i].push_back(std::get<2>(item));
control_modes[i].erase(
std::remove_if(control_modes[i].begin(), control_modes[i].end(),
[&item](const std::string& entry) { return entry == std::get<1>(item); }),
control_modes[i].end());
}
}
}
}

// Do not start conflicting controllers
// Passthrough controller requested to start
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) { return (item == PASSTHROUGH_GPIO); }) &&
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == FREEDRIVE_MODE_GPIO);
}) ||
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == FREEDRIVE_MODE_GPIO);
}))) {
RCLCPP_ERROR(get_logger(), "Attempting to start passthrough_trajectory control while there is either position or "
"velocity or freedrive mode running.");
ret_val = hardware_interface::return_type::ERROR;
}

// Force mode requested to start
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) { return (item == FORCE_MODE_GPIO); }) &&
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
}) ||
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
}))) {
RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
"velocity mode running.");
ret_val = hardware_interface::return_type::ERROR;
}

// Freedrive mode requested to start
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) &&
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
}) ||
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
}))) {
RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
"velocity mode running.");
ret_val = hardware_interface::return_type::ERROR;
}

// Tool contact controller requested to start
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) { return (item == TOOL_CONTACT_GPIO); }) &&
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) ||
std::any_of(control_modes[0].begin(), control_modes[0].end(),
[this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) {
RCLCPP_ERROR(get_logger(), "Attempting to start tool contact controller while either the force mode controller or "
"the freedrive controller is running.");
ret_val = hardware_interface::return_type::ERROR;
}

// Position mode requested to start
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) &&
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
}) ||
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
}))) {
RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is either trajectory passthrough or "
"velocity mode or force_mode or freedrive mode running.");
ret_val = hardware_interface::return_type::ERROR;
}

// Velocity mode requested to start
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) &&
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) {
return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
}) ||
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
}))) {
RCLCPP_ERROR(get_logger(), "Attempting to start velosity control while there is either trajectory passthrough or "
"position mode or force_mode or freedrive mode running.");
ret_val = hardware_interface::return_type::ERROR;
for (auto& start_mode : start_modes_[0]) {
if (!is_mode_compatible(start_mode, control_modes[0])) {
return hardware_interface::return_type::ERROR;
}
}

controllers_initialized_ = true;
Expand Down
Loading