Skip to content

Commit 0feddae

Browse files
urfeexmergify[bot]
urfeex
authored and
mergify[bot]
committed
Refactor prepare_switch method (#1417)
Refactored prepare_switch mechanism. This way it should be easier to understand and to extend. (cherry picked from commit effc0a0)
1 parent b3be185 commit 0feddae

File tree

2 files changed

+97
-187
lines changed

2 files changed

+97
-187
lines changed

‎ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,11 @@
4040
#define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_
4141

4242
// System
43+
#include <limits>
4344
#include <memory>
4445
#include <string>
46+
#include <unordered_map>
4547
#include <vector>
46-
#include <limits>
4748

4849
// ros2_control hardware_interface
4950
#include "hardware_interface/hardware_info.hpp"
@@ -114,6 +115,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
114115
{
115116
public:
116117
RCLCPP_SHARED_PTR_DEFINITIONS(URPositionHardwareInterface);
118+
URPositionHardwareInterface();
117119
virtual ~URPositionHardwareInterface();
118120

119121
hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) final;
@@ -316,6 +318,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
316318
const std::string FORCE_MODE_GPIO = "force_mode";
317319
const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode";
318320
const std::string TOOL_CONTACT_GPIO = "tool_contact";
321+
322+
std::unordered_map<std::string, std::unordered_map<std::string, bool>> mode_compatibility_;
319323
};
320324
} // namespace ur_robot_driver
321325

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 92 additions & 186 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,45 @@ namespace rtde = urcl::rtde_interface;
5757
namespace ur_robot_driver
5858
{
5959

60+
URPositionHardwareInterface::URPositionHardwareInterface()
61+
{
62+
mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_VELOCITY] = false;
63+
mode_compatibility_[hardware_interface::HW_IF_POSITION][FORCE_MODE_GPIO] = false;
64+
mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false;
65+
mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false;
66+
mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true;
67+
68+
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false;
69+
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false;
70+
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false;
71+
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false;
72+
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true;
73+
74+
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
75+
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
76+
mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true;
77+
mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false;
78+
mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
79+
80+
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false;
81+
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
82+
mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true;
83+
mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false;
84+
mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true;
85+
86+
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
87+
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
88+
mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false;
89+
mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false;
90+
mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
91+
92+
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true;
93+
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true;
94+
mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false;
95+
mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true;
96+
mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false;
97+
}
98+
6099
URPositionHardwareInterface::~URPositionHardwareInterface()
61100
{
62101
}
@@ -1099,66 +1138,41 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
10991138
}
11001139
}
11011140

1141+
auto is_mode_compatible = [this](const std::string& mode, const std::vector<std::string>& other_modes) {
1142+
for (auto& other : other_modes) {
1143+
if (mode == other)
1144+
continue;
1145+
1146+
if (mode_compatibility_[mode][other] == false) {
1147+
RCLCPP_ERROR(get_logger(), "Starting %s together with %s is not allowed. ", mode.c_str(), other.c_str());
1148+
return false;
1149+
}
1150+
}
1151+
return true;
1152+
};
1153+
11021154
// Starting interfaces
11031155
// If a joint has been reserved already, raise an error.
11041156
// Modes that are not directly mapped to a single joint such as force_mode reserve all joints.
11051157
for (const auto& key : start_interfaces) {
11061158
for (auto i = 0u; i < info_.joints.size(); i++) {
1107-
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
1108-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1109-
return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
1110-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
1111-
})) {
1112-
RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is another control mode already "
1113-
"requested.");
1114-
return hardware_interface::return_type::ERROR;
1115-
}
1116-
start_modes_[i].push_back(hardware_interface::HW_IF_POSITION);
1117-
} else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
1118-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1119-
return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
1120-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
1121-
})) {
1122-
RCLCPP_ERROR(get_logger(), "Attempting to start velocity control while there is another control mode already "
1123-
"requested.");
1124-
return hardware_interface::return_type::ERROR;
1125-
}
1126-
start_modes_[i].push_back(hardware_interface::HW_IF_VELOCITY);
1127-
} else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
1128-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1129-
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
1130-
})) {
1131-
RCLCPP_ERROR(get_logger(), "Attempting to start force_mode control while there is either position or "
1132-
"velocity mode already requested by another controller.");
1133-
return hardware_interface::return_type::ERROR;
1134-
}
1135-
start_modes_[i].push_back(FORCE_MODE_GPIO);
1136-
} else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
1137-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1138-
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
1139-
})) {
1140-
RCLCPP_ERROR(get_logger(), "Attempting to start trajectory passthrough control while there is either "
1141-
"position or velocity mode already requested by another controller.");
1142-
return hardware_interface::return_type::ERROR;
1143-
}
1144-
start_modes_[i].push_back(PASSTHROUGH_GPIO);
1145-
} else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
1146-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1147-
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY ||
1148-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO;
1149-
})) {
1150-
return hardware_interface::return_type::ERROR;
1151-
}
1152-
start_modes_[i].push_back(FREEDRIVE_MODE_GPIO);
1153-
} else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") {
1154-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1155-
return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
1156-
})) {
1157-
RCLCPP_ERROR(get_logger(), "Attempting to start tool contact controller while either the force mode or "
1158-
"freedrive controller is running.");
1159-
return hardware_interface::return_type::ERROR;
1159+
const std::vector<std::pair<std::string, std::string>> start_modes_to_check{
1160+
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION },
1161+
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY },
1162+
{ tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO },
1163+
{ tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO },
1164+
{ tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO },
1165+
{ tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO }
1166+
};
1167+
1168+
for (auto& item : start_modes_to_check) {
1169+
if (key == item.first) {
1170+
if (!is_mode_compatible(item.second, start_modes_[i])) {
1171+
return hardware_interface::return_type::ERROR;
1172+
}
1173+
start_modes_[i].push_back(item.second);
1174+
continue;
11601175
}
1161-
start_modes_[i].push_back(TOOL_CONTACT_GPIO);
11621176
}
11631177
}
11641178
}
@@ -1173,143 +1187,35 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
11731187
// add stop interface per joint in tmp var for later check
11741188
for (const auto& key : stop_interfaces) {
11751189
for (auto i = 0u; i < info_.joints.size(); i++) {
1176-
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
1177-
stop_modes_[i].push_back(StoppingInterface::STOP_POSITION);
1178-
control_modes[i].erase(
1179-
std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1180-
[](const std::string& item) { return item == hardware_interface::HW_IF_POSITION; }),
1181-
control_modes[i].end());
1182-
}
1183-
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
1184-
stop_modes_[i].push_back(StoppingInterface::STOP_VELOCITY);
1185-
control_modes[i].erase(
1186-
std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1187-
[](const std::string& item) { return item == hardware_interface::HW_IF_VELOCITY; }),
1188-
control_modes[i].end());
1189-
}
1190-
if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") {
1191-
stop_modes_[i].push_back(StoppingInterface::STOP_FORCE_MODE);
1192-
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1193-
[&](const std::string& item) { return item == FORCE_MODE_GPIO; }),
1194-
control_modes[i].end());
1195-
}
1196-
if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
1197-
stop_modes_[i].push_back(StoppingInterface::STOP_PASSTHROUGH);
1198-
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1199-
[&](const std::string& item) { return item == PASSTHROUGH_GPIO; }),
1200-
control_modes[i].end());
1201-
}
1202-
if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
1203-
stop_modes_[i].push_back(StoppingInterface::STOP_FREEDRIVE);
1204-
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1205-
[&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }),
1206-
control_modes[i].end());
1207-
}
1208-
if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") {
1209-
stop_modes_[i].push_back(StoppingInterface::STOP_TOOL_CONTACT);
1210-
control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1211-
[&](const std::string& item) { return item == TOOL_CONTACT_GPIO; }),
1212-
control_modes[i].end());
1190+
const std::vector<std::tuple<std::string, std::string, StoppingInterface>> stop_modes_to_check{
1191+
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION,
1192+
StoppingInterface::STOP_POSITION },
1193+
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY,
1194+
StoppingInterface::STOP_VELOCITY },
1195+
{ tf_prefix + FORCE_MODE_GPIO + "/disable_cmd", FORCE_MODE_GPIO, StoppingInterface::STOP_FORCE_MODE },
1196+
{ tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO,
1197+
StoppingInterface::STOP_PASSTHROUGH },
1198+
{ tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO, StoppingInterface::STOP_FREEDRIVE },
1199+
{ tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO,
1200+
StoppingInterface::STOP_TOOL_CONTACT }
1201+
};
1202+
for (auto& item : stop_modes_to_check) {
1203+
if (key == std::get<0>(item)) {
1204+
stop_modes_[i].push_back(std::get<2>(item));
1205+
control_modes[i].erase(
1206+
std::remove_if(control_modes[i].begin(), control_modes[i].end(),
1207+
[&item](const std::string& entry) { return entry == std::get<1>(item); }),
1208+
control_modes[i].end());
1209+
}
12131210
}
12141211
}
12151212
}
12161213

12171214
// Do not start conflicting controllers
1218-
// Passthrough controller requested to start
1219-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1220-
[this](auto& item) { return (item == PASSTHROUGH_GPIO); }) &&
1221-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1222-
[this](auto& item) {
1223-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1224-
item == FREEDRIVE_MODE_GPIO);
1225-
}) ||
1226-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1227-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1228-
item == FREEDRIVE_MODE_GPIO);
1229-
}))) {
1230-
RCLCPP_ERROR(get_logger(), "Attempting to start passthrough_trajectory control while there is either position or "
1231-
"velocity or freedrive mode running.");
1232-
ret_val = hardware_interface::return_type::ERROR;
1233-
}
1234-
1235-
// Force mode requested to start
1236-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1237-
[this](auto& item) { return (item == FORCE_MODE_GPIO); }) &&
1238-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1239-
[this](auto& item) {
1240-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1241-
item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1242-
}) ||
1243-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1244-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1245-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1246-
}))) {
1247-
RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
1248-
"velocity mode running.");
1249-
ret_val = hardware_interface::return_type::ERROR;
1250-
}
1251-
1252-
// Freedrive mode requested to start
1253-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1254-
[this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) &&
1255-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1256-
[this](auto& item) {
1257-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1258-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1259-
}) ||
1260-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1261-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1262-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1263-
}))) {
1264-
RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
1265-
"velocity mode running.");
1266-
ret_val = hardware_interface::return_type::ERROR;
1267-
}
1268-
1269-
// Tool contact controller requested to start
1270-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1271-
[this](auto& item) { return (item == TOOL_CONTACT_GPIO); }) &&
1272-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1273-
[this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) ||
1274-
std::any_of(control_modes[0].begin(), control_modes[0].end(),
1275-
[this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) {
1276-
RCLCPP_ERROR(get_logger(), "Attempting to start tool contact controller while either the force mode controller or "
1277-
"the freedrive controller is running.");
1278-
ret_val = hardware_interface::return_type::ERROR;
1279-
}
1280-
1281-
// Position mode requested to start
1282-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1283-
[](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) &&
1284-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1285-
[this](auto& item) {
1286-
return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
1287-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1288-
}) ||
1289-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1290-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1291-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1292-
}))) {
1293-
RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is either trajectory passthrough or "
1294-
"velocity mode or force_mode or freedrive mode running.");
1295-
ret_val = hardware_interface::return_type::ERROR;
1296-
}
1297-
1298-
// Velocity mode requested to start
1299-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1300-
[](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) &&
1301-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1302-
[this](auto& item) {
1303-
return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
1304-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1305-
}) ||
1306-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1307-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1308-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1309-
}))) {
1310-
RCLCPP_ERROR(get_logger(), "Attempting to start velosity control while there is either trajectory passthrough or "
1311-
"position mode or force_mode or freedrive mode running.");
1312-
ret_val = hardware_interface::return_type::ERROR;
1215+
for (auto& start_mode : start_modes_[0]) {
1216+
if (!is_mode_compatible(start_mode, control_modes[0])) {
1217+
return hardware_interface::return_type::ERROR;
1218+
}
13131219
}
13141220

13151221
controllers_initialized_ = true;

0 commit comments

Comments
 (0)