From 99943b97fe7d8be421d77e916323e72c39dc2a82 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Apr 2025 11:45:59 +0200 Subject: [PATCH] Only append to start_modes in prepare_switch Before the main control mode (position/velocity) was always overwriting the complete list for the joint. That worked fine when that was the only control mode for a joint or if it was the first in the list. But that is not guaranteed. Since we create the list as an empty vector beforehand, anyway, we can simply append to that list in any case. The list will be checked for compatibility at a later point, anyway. --- ur_robot_driver/src/hardware_interface.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 621321e61..a31ed1f8d 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -1099,12 +1099,6 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } } - if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), - [&](const std::vector& other) { return other == start_modes_[0]; })) { - RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same."); - return hardware_interface::return_type::ERROR; - } - // 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. @@ -1119,7 +1113,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = { hardware_interface::HW_IF_POSITION }; + 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 || @@ -1129,7 +1123,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = { hardware_interface::HW_IF_VELOCITY }; + 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; @@ -1169,6 +1163,12 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } } + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), + [&](const std::vector& other) { return other == start_modes_[0]; })) { + RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same."); + return hardware_interface::return_type::ERROR; + } + // Stopping interfaces // add stop interface per joint in tmp var for later check for (const auto& key : stop_interfaces) {