Skip to content

Commit 786b888

Browse files
urfeexurfeex
committed
Resolve conflicts
1 parent c1bcc12 commit 786b888

File tree

1 file changed

+2
-172
lines changed

1 file changed

+2
-172
lines changed

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 2 additions & 172 deletions
Original file line numberDiff line numberDiff line change
@@ -1139,7 +1139,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
11391139
continue;
11401140

11411141
if (mode_compatibility_[mode][other] == false) {
1142-
RCLCPP_ERROR(get_logger(), "Starting %s together with %s is not allowed. ", mode.c_str(), other.c_str());
1142+
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Starting %s together with %s is not allowed. ",
1143+
mode.c_str(), other.c_str());
11431144
return false;
11441145
}
11451146
}
@@ -1151,70 +1152,6 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
11511152
// Modes that are not directly mapped to a single joint such as force_mode reserve all joints.
11521153
for (const auto& key : start_interfaces) {
11531154
for (auto i = 0u; i < info_.joints.size(); i++) {
1154-
<<<<<<< HEAD
1155-
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
1156-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1157-
return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
1158-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
1159-
})) {
1160-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while "
1161-
"there is another control mode already "
1162-
"requested.");
1163-
return hardware_interface::return_type::ERROR;
1164-
}
1165-
start_modes_[i].push_back(hardware_interface::HW_IF_POSITION);
1166-
} else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
1167-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1168-
return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
1169-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
1170-
})) {
1171-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while "
1172-
"there is another control mode already "
1173-
"requested.");
1174-
return hardware_interface::return_type::ERROR;
1175-
}
1176-
start_modes_[i].push_back(hardware_interface::HW_IF_VELOCITY);
1177-
} else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
1178-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1179-
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
1180-
})) {
1181-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force_mode control "
1182-
"while there is either position or "
1183-
"velocity mode already requested by another "
1184-
"controller.");
1185-
return hardware_interface::return_type::ERROR;
1186-
}
1187-
start_modes_[i].push_back(FORCE_MODE_GPIO);
1188-
} else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
1189-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1190-
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
1191-
})) {
1192-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start trajectory passthrough "
1193-
"control while there is either "
1194-
"position or velocity mode already requested "
1195-
"by another controller.");
1196-
return hardware_interface::return_type::ERROR;
1197-
}
1198-
start_modes_[i].push_back(PASSTHROUGH_GPIO);
1199-
} else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
1200-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1201-
return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY ||
1202-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO;
1203-
})) {
1204-
return hardware_interface::return_type::ERROR;
1205-
}
1206-
start_modes_[i].push_back(FREEDRIVE_MODE_GPIO);
1207-
} else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") {
1208-
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
1209-
return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
1210-
})) {
1211-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller "
1212-
"while either the force mode or "
1213-
"freedrive controller is running.");
1214-
return hardware_interface::return_type::ERROR;
1215-
}
1216-
start_modes_[i].push_back(TOOL_CONTACT_GPIO);
1217-
=======
12181155
const std::vector<std::pair<std::string, std::string>> start_modes_to_check{
12191156
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION },
12201157
{ info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY },
@@ -1232,7 +1169,6 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
12321169
start_modes_[i].push_back(item.second);
12331170
continue;
12341171
}
1235-
>>>>>>> effc0a0 (Refactor prepare_switch method (#1417))
12361172
}
12371173
}
12381174
}
@@ -1272,116 +1208,10 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
12721208
}
12731209

12741210
// Do not start conflicting controllers
1275-
<<<<<<< HEAD
1276-
// Passthrough controller requested to start
1277-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1278-
[this](auto& item) { return (item == PASSTHROUGH_GPIO); }) &&
1279-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1280-
[this](auto& item) {
1281-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1282-
item == FREEDRIVE_MODE_GPIO);
1283-
}) ||
1284-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1285-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1286-
item == FREEDRIVE_MODE_GPIO);
1287-
}))) {
1288-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory "
1289-
"control while there is either position or "
1290-
"velocity or freedrive mode running.");
1291-
ret_val = hardware_interface::return_type::ERROR;
1292-
}
1293-
1294-
// Force mode requested to start
1295-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1296-
[this](auto& item) { return (item == FORCE_MODE_GPIO); }) &&
1297-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1298-
[this](auto& item) {
1299-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1300-
item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1301-
}) ||
1302-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1303-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1304-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1305-
}))) {
1306-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while "
1307-
"there is either position or "
1308-
"velocity mode running.");
1309-
ret_val = hardware_interface::return_type::ERROR;
1310-
}
1311-
1312-
// Freedrive mode requested to start
1313-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1314-
[this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) &&
1315-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1316-
[this](auto& item) {
1317-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1318-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1319-
}) ||
1320-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1321-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1322-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
1323-
}))) {
1324-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while "
1325-
"there is either position or "
1326-
"velocity mode running.");
1327-
ret_val = hardware_interface::return_type::ERROR;
1328-
}
1329-
1330-
// Tool contact controller requested to start
1331-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1332-
[this](auto& item) { return (item == TOOL_CONTACT_GPIO); }) &&
1333-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1334-
[this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) ||
1335-
std::any_of(control_modes[0].begin(), control_modes[0].end(),
1336-
[this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) {
1337-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller while "
1338-
"either the force mode controller or "
1339-
"the freedrive controller is running.");
1340-
ret_val = hardware_interface::return_type::ERROR;
1341-
}
1342-
1343-
// Position mode requested to start
1344-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1345-
[](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) &&
1346-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1347-
[this](auto& item) {
1348-
return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
1349-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1350-
}) ||
1351-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1352-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1353-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1354-
}))) {
1355-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there "
1356-
"is either trajectory passthrough or "
1357-
"velocity mode or force_mode or freedrive mode "
1358-
"running.");
1359-
ret_val = hardware_interface::return_type::ERROR;
1360-
}
1361-
1362-
// Velocity mode requested to start
1363-
if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1364-
[](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) &&
1365-
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
1366-
[this](auto& item) {
1367-
return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
1368-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1369-
}) ||
1370-
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
1371-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1372-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1373-
}))) {
1374-
RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while there "
1375-
"is either trajectory passthrough or "
1376-
"position mode or force_mode or freedrive mode "
1377-
"running.");
1378-
ret_val = hardware_interface::return_type::ERROR;
1379-
=======
13801211
for (auto& start_mode : start_modes_[0]) {
13811212
if (!is_mode_compatible(start_mode, control_modes[0])) {
13821213
return hardware_interface::return_type::ERROR;
13831214
}
1384-
>>>>>>> effc0a0 (Refactor prepare_switch method (#1417))
13851215
}
13861216

13871217
controllers_initialized_ = true;

0 commit comments

Comments
 (0)