Skip to content

Commit efbb144

Browse files
urfeexurfeex
committed
Fix interface compatibility check in prepare_switch
Also added tests for that
1 parent d8b533c commit efbb144

File tree

4 files changed

+366
-38
lines changed

4 files changed

+366
-38
lines changed

‎ur_robot_driver/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,10 @@ if(BUILD_TESTING)
188188
TIMEOUT
189189
800
190190
)
191+
add_launch_test(test/integration_test_controller_switch.py
192+
TIMEOUT
193+
800
194+
)
191195
add_launch_test(test/urscript_interface.py
192196
TIMEOUT
193197
500

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 73 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -876,24 +876,52 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
876876
{
877877
hardware_interface::return_type ret_val = hardware_interface::return_type::OK;
878878

879-
start_modes_.clear();
879+
start_modes_ = std::vector<std::string>(info_.joints.size(), "UNDEFINED");
880880
stop_modes_.clear();
881+
std::vector<std::string> control_modes(info_.joints.size());
881882
const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix");
882883

884+
// Assess current state
885+
for (auto i = 0u; i < info_.joints.size(); i++) {
886+
if (position_controller_running_) {
887+
control_modes[i] = hardware_interface::HW_IF_POSITION;
888+
}
889+
if (velocity_controller_running_) {
890+
control_modes[i] = hardware_interface::HW_IF_VELOCITY;
891+
}
892+
if (force_mode_controller_running_) {
893+
control_modes[i] = FORCE_MODE_GPIO;
894+
}
895+
}
896+
897+
if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
898+
[&](const std::string& other) { return other == start_modes_[0]; })) {
899+
RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");
900+
return hardware_interface::return_type::ERROR;
901+
}
902+
883903
// Starting interfaces
884-
// add start interface per joint in tmp var for later check
904+
// If a joint has been reserved already, raise an error.
905+
// Modes that are not directly mapped to a single joint such as force_mode reserve all joints.
885906
for (const auto& key : start_interfaces) {
886907
for (auto i = 0u; i < info_.joints.size(); i++) {
887908
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
888-
start_modes_.push_back(hardware_interface::HW_IF_POSITION);
889-
}
890-
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
891-
start_modes_.push_back(hardware_interface::HW_IF_VELOCITY);
909+
if (start_modes_[i] != "UNDEFINED") {
910+
return hardware_interface::return_type::ERROR;
911+
}
912+
start_modes_[i] = hardware_interface::HW_IF_POSITION;
913+
} else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
914+
if (start_modes_[i] != "UNDEFINED") {
915+
return hardware_interface::return_type::ERROR;
916+
}
917+
start_modes_[i] = hardware_interface::HW_IF_VELOCITY;
918+
} else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
919+
if (start_modes_[i] != "UNDEFINED") {
920+
return hardware_interface::return_type::ERROR;
921+
}
922+
start_modes_[i] = FORCE_MODE_GPIO;
892923
}
893924
}
894-
if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
895-
start_modes_.push_back(FORCE_MODE_GPIO);
896-
}
897925
}
898926

899927
// Stopping interfaces
@@ -902,46 +930,54 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
902930
for (auto i = 0u; i < info_.joints.size(); i++) {
903931
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
904932
stop_modes_.push_back(StoppingInterface::STOP_POSITION);
933+
if (control_modes[i] == hardware_interface::HW_IF_POSITION) {
934+
control_modes[i] = "UNDEFINED";
935+
}
905936
}
906937
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
907938
stop_modes_.push_back(StoppingInterface::STOP_VELOCITY);
939+
if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) {
940+
control_modes[i] = "UNDEFINED";
941+
}
942+
}
943+
if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") {
944+
stop_modes_.push_back(StoppingInterface::STOP_FORCE_MODE);
945+
if (control_modes[i] == FORCE_MODE_GPIO) {
946+
control_modes[i] = "UNDEFINED";
947+
}
908948
}
909-
}
910-
if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") {
911-
stop_modes_.push_back(StoppingInterface::STOP_FORCE_MODE);
912949
}
913950
}
914951

915-
if (position_controller_running_ &&
916-
std::none_of(stop_modes_.begin(), stop_modes_.end(),
917-
[](auto item) { return item == StoppingInterface::STOP_POSITION; }) &&
918-
std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
919-
return (item == hardware_interface::HW_IF_VELOCITY || item == FORCE_MODE_GPIO);
920-
})) {
921-
RCLCPP_ERROR(get_logger(), "Start of velocity or force_mode interface requested while there is the position "
922-
"interface running.");
952+
// Do not start conflicting controllers
953+
if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FORCE_MODE_GPIO); }) &&
954+
(std::any_of(start_modes_.begin(), start_modes_.end(),
955+
[](auto& item) {
956+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
957+
}) ||
958+
std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
959+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
960+
item == FORCE_MODE_GPIO);
961+
}))) {
923962
ret_val = hardware_interface::return_type::ERROR;
924963
}
925-
926-
if (velocity_controller_running_ &&
927-
std::none_of(stop_modes_.begin(), stop_modes_.end(),
928-
[](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) &&
929-
std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
930-
return (item == hardware_interface::HW_IF_POSITION || item == FORCE_MODE_GPIO);
931-
})) {
932-
RCLCPP_ERROR(get_logger(), "Start of position or force_mode interface requested while there is the velocity "
933-
"interface running.");
964+
if (std::any_of(start_modes_.begin(), start_modes_.end(),
965+
[](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) &&
966+
(std::any_of(
967+
start_modes_.begin(), start_modes_.end(),
968+
[this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == FORCE_MODE_GPIO); }) ||
969+
std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
970+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
971+
item == FORCE_MODE_GPIO);
972+
}))) {
934973
ret_val = hardware_interface::return_type::ERROR;
935974
}
936-
937-
if (force_mode_controller_running_ &&
938-
std::none_of(stop_modes_.begin(), stop_modes_.end(),
939-
[](auto item) { return item == StoppingInterface::STOP_FORCE_MODE; }) &&
940-
std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) {
941-
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
975+
if (std::any_of(start_modes_.begin(), start_modes_.end(),
976+
[](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) &&
977+
std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
978+
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
979+
item == FORCE_MODE_GPIO);
942980
})) {
943-
RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while force_mode "
944-
"controller is running.");
945981
ret_val = hardware_interface::return_type::ERROR;
946982
}
947983

0 commit comments

Comments
 (0)