Skip to content

Commit 3f0767d

Browse files
urfeexurfeex
committed
Fix interface compatibility check in prepare_switch (with tests)
This makes sure that it is not possible to activate more than one writing controller at the same time (independent whether on tries to start them together or one of them is already running) I've also added a test for that to ensure that what we want is actually achieved.
1 parent d706fa0 commit 3f0767d

File tree

4 files changed

+374
-52
lines changed

4 files changed

+374
-52
lines changed

‎ur_robot_driver/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -184,6 +184,10 @@ if(BUILD_TESTING)
184184
TIMEOUT
185185
800
186186
)
187+
add_launch_test(test/integration_test_controller_switch.py
188+
TIMEOUT
189+
800
190+
)
187191
add_launch_test(test/urscript_interface.py
188192
TIMEOUT
189193
500

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 72 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -866,21 +866,51 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
866866
const std::vector<std::string>& start_interfaces, const std::vector<std::string>& stop_interfaces)
867867
{
868868
hardware_interface::return_type ret_val = hardware_interface::return_type::OK;
869-
start_modes_.clear();
869+
870+
start_modes_ = std::vector<std::string>(info_.joints.size(), "UNDEFINED");
870871
stop_modes_.clear();
872+
std::vector<std::string> control_modes(info_.joints.size());
871873
const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix");
874+
875+
// Assess current state
876+
for (auto i = 0u; i < info_.joints.size(); i++) {
877+
if (position_controller_running_) {
878+
control_modes[i] = hardware_interface::HW_IF_POSITION;
879+
}
880+
if (velocity_controller_running_) {
881+
control_modes[i] = hardware_interface::HW_IF_VELOCITY;
882+
}
883+
if (passthrough_trajectory_controller_running_) {
884+
control_modes[i] = PASSTHROUGH_GPIO;
885+
}
886+
}
887+
888+
if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
889+
[&](const std::string& other) { return other == start_modes_[0]; })) {
890+
RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");
891+
return hardware_interface::return_type::ERROR;
892+
}
893+
872894
// Starting interfaces
873-
// add start interface per joint in tmp var for later check
895+
// If a joint has been reserved already, raise an error.
896+
// Modes that are not directly mapped to a single joint such as force_mode reserve all joints.
874897
for (const auto& key : start_interfaces) {
875898
for (auto i = 0u; i < info_.joints.size(); i++) {
876899
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
877-
start_modes_.push_back(hardware_interface::HW_IF_POSITION);
878-
}
879-
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
880-
start_modes_.push_back(hardware_interface::HW_IF_VELOCITY);
881-
}
882-
if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
883-
start_modes_.push_back(PASSTHROUGH_GPIO);
900+
if (start_modes_[i] != "UNDEFINED") {
901+
return hardware_interface::return_type::ERROR;
902+
}
903+
start_modes_[i] = hardware_interface::HW_IF_POSITION;
904+
} else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
905+
if (start_modes_[i] != "UNDEFINED") {
906+
return hardware_interface::return_type::ERROR;
907+
}
908+
start_modes_[i] = hardware_interface::HW_IF_VELOCITY;
909+
} else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
910+
if (start_modes_[i] != "UNDEFINED") {
911+
return hardware_interface::return_type::ERROR;
912+
}
913+
start_modes_[i] = PASSTHROUGH_GPIO;
884914
}
885915
}
886916
}
@@ -891,65 +921,55 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
891921
for (auto i = 0u; i < info_.joints.size(); i++) {
892922
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
893923
stop_modes_.push_back(StoppingInterface::STOP_POSITION);
924+
if (control_modes[i] == hardware_interface::HW_IF_POSITION) {
925+
control_modes[i] = "UNDEFINED";
926+
}
894927
}
895928
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
896929
stop_modes_.push_back(StoppingInterface::STOP_VELOCITY);
930+
if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) {
931+
control_modes[i] = "UNDEFINED";
932+
}
897933
}
898934
if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
899935
stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH);
936+
if (control_modes[i] == PASSTHROUGH_GPIO) {
937+
control_modes[i] = "UNDEFINED";
938+
}
900939
}
901940
}
902941
}
903942

904-
// set new mode to all interfaces at the same time
905-
if (start_modes_.size() != 0 && start_modes_.size() != 6) {
906-
RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be started at once.");
943+
// Do not start conflicting controllers
944+
if (std::any_of(start_modes_.begin(), start_modes_.end(),
945+
[this](auto& item) { return (item == PASSTHROUGH_GPIO); }) &&
946+
(std::any_of(start_modes_.begin(), start_modes_.end(),
947+
[](auto& item) {
948+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
949+
}) ||
950+
std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
951+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
952+
item == PASSTHROUGH_GPIO);
953+
}))) {
907954
ret_val = hardware_interface::return_type::ERROR;
908955
}
909-
910-
if (position_controller_running_ &&
911-
std::none_of(stop_modes_.begin(), stop_modes_.end(),
912-
[](auto item) { return item == StoppingInterface::STOP_POSITION; }) &&
913-
std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
914-
return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO);
915-
})) {
916-
RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position "
917-
"interface running.");
956+
if (std::any_of(start_modes_.begin(), start_modes_.end(),
957+
[](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) &&
958+
(std::any_of(
959+
start_modes_.begin(), start_modes_.end(),
960+
[this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) ||
961+
std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
962+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
963+
item == PASSTHROUGH_GPIO);
964+
}))) {
918965
ret_val = hardware_interface::return_type::ERROR;
919966
}
920-
921-
if (velocity_controller_running_ &&
922-
std::none_of(stop_modes_.begin(), stop_modes_.end(),
923-
[](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) &&
967+
if (std::any_of(start_modes_.begin(), start_modes_.end(),
968+
[](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) &&
924969
std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
925-
return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO);
926-
})) {
927-
RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity "
928-
"interface running.");
929-
ret_val = hardware_interface::return_type::ERROR;
930-
}
931-
932-
if (passthrough_trajectory_controller_running_ &&
933-
std::none_of(stop_modes_.begin(), stop_modes_.end(),
934-
[](auto item) { return item == StoppingInterface::STOP_PASSTHROUGH; }) &&
935-
std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) {
936-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
970+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
971+
item == PASSTHROUGH_GPIO);
937972
})) {
938-
RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while there is the passthrough "
939-
"interface running.");
940-
ret_val = hardware_interface::return_type::ERROR;
941-
}
942-
943-
// all start interfaces must be the same - can't mix position and velocity control
944-
if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) {
945-
RCLCPP_ERROR(get_logger(), "Can't mix different control domains for joint control.");
946-
ret_val = hardware_interface::return_type::ERROR;
947-
}
948-
949-
// stop all interfaces at the same time
950-
if (stop_modes_.size() != 0 &&
951-
(stop_modes_.size() != 6 || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) {
952-
RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be stopped at once.");
953973
ret_val = hardware_interface::return_type::ERROR;
954974
}
955975

0 commit comments

Comments
 (0)