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
+97 −187
Merged

Refactor prepare_switch method #1417

Show file tree
Hide file tree
Changes from all commits
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 ur_robot_driver
{

URPositionHardwareInterface::URPositionHardwareInterface()

Check warning on line 60 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L60 

Added line #L60 was not covered by tests
{
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;

Check warning on line 66 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L62-L66 

Added lines #L62 - L66 were not covered by tests

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;

Check warning on line 72 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L68-L72 

Added lines #L68 - L72 were not covered by tests

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;

Check warning on line 78 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L74-L78 

Added lines #L74 - L78 were not covered by tests

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;

Check warning on line 84 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L80-L84 

Added lines #L80 - L84 were not covered by tests

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;

Check warning on line 90 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L86-L90 

Added lines #L86 - L90 were not covered by tests

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;
}

Check warning on line 97 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L92-L97 

Added lines #L92 - L97 were not covered by tests

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

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

Check warning on line 1141 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1141 

Added line #L1141 was not covered by tests
for (auto& other : other_modes) {
if (mode == other)
continue;

Check warning on line 1144 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1144 

Added line #L1144 was not covered by tests

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;

Check warning on line 1148 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1147-L1148 

Added lines #L1147 - L1148 were not covered by tests
}
}
return true;
};

Check warning on line 1152 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1152 

Added line #L1152 was not covered by tests

// 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;
const 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 }
};

Check warning on line 1166 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1159-L1166 

Added lines #L1159 - L1166 were not covered by tests

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;

Check warning on line 1171 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1171 

Added line #L1171 was not covered by tests
}
start_modes_[i].push_back(item.second);
continue;

Check warning on line 1174 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1173-L1174 

Added lines #L1173 - L1174 were not covered by tests
}
start_modes_[i].push_back(TOOL_CONTACT_GPIO);
}
}
}
Expand All @@ -1173,143 +1187,35 @@
// 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());
const 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 }
};

Check warning on line 1201 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1190-L1201 

Added lines #L1190 - L1201 were not covered by tests
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());

Check warning on line 1208 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1204-L1208 

Added lines #L1204 - L1208 were not covered by tests
}
}
}
}

// 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;

Check warning on line 1217 in ur_robot_driver/src/hardware_interface.cpp

View check run for this annotation

Codecov / codecov/patch

ur_robot_driver/src/hardware_interface.cpp#L1217 

Added line #L1217 was not covered by tests
}
}

controllers_initialized_ = true;
Expand Down
Loading