From 3f0767d953b9b3491ba2afe423fd4ac256876e40 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 11 Nov 2024 09:18:51 +0100 Subject: [PATCH] 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. --- ur_robot_driver/CMakeLists.txt | 4 + ur_robot_driver/src/hardware_interface.cpp | 124 ++++---- .../integration_test_controller_switch.py | 289 ++++++++++++++++++ ur_robot_driver/test/test_common.py | 9 + 4 files changed, 374 insertions(+), 52 deletions(-) create mode 100644 ur_robot_driver/test/integration_test_controller_switch.py diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c47a83452..0d0487a03 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -184,6 +184,10 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_controller_switch.py + TIMEOUT + 800 + ) add_launch_test(test/urscript_interface.py TIMEOUT 500 diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 03da69f7a..ca05c45bd 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -866,21 +866,51 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod const std::vector& start_interfaces, const std::vector& stop_interfaces) { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_.clear(); + + start_modes_ = std::vector(info_.joints.size(), "UNDEFINED"); stop_modes_.clear(); + std::vector control_modes(info_.joints.size()); const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); + + // Assess current state + for (auto i = 0u; i < info_.joints.size(); i++) { + if (position_controller_running_) { + control_modes[i] = hardware_interface::HW_IF_POSITION; + } + if (velocity_controller_running_) { + control_modes[i] = hardware_interface::HW_IF_VELOCITY; + } + if (passthrough_trajectory_controller_running_) { + control_modes[i] = PASSTHROUGH_GPIO; + } + } + + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), + [&](const std::string& 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 - // add start interface per joint in tmp var for later check + // 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. for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - start_modes_.push_back(hardware_interface::HW_IF_POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); - } - if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - start_modes_.push_back(PASSTHROUGH_GPIO); + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = hardware_interface::HW_IF_POSITION; + } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = hardware_interface::HW_IF_VELOCITY; + } else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = PASSTHROUGH_GPIO; } } } @@ -891,65 +921,55 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { stop_modes_.push_back(StoppingInterface::STOP_POSITION); + if (control_modes[i] == hardware_interface::HW_IF_POSITION) { + control_modes[i] = "UNDEFINED"; + } } if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); + if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) { + control_modes[i] = "UNDEFINED"; + } } if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); + if (control_modes[i] == PASSTHROUGH_GPIO) { + control_modes[i] = "UNDEFINED"; + } } } } - // set new mode to all interfaces at the same time - if (start_modes_.size() != 0 && start_modes_.size() != 6) { - RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be started at once."); + // Do not start conflicting controllers + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && + (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + }))) { ret_val = hardware_interface::return_type::ERROR; } - - if (position_controller_running_ && - std::none_of(stop_modes_.begin(), stop_modes_.end(), - [](auto item) { return item == StoppingInterface::STOP_POSITION; }) && - std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); - })) { - RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position " - "interface running."); + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && + (std::any_of( + start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + }))) { ret_val = hardware_interface::return_type::ERROR; } - - if (velocity_controller_running_ && - std::none_of(stop_modes_.begin(), stop_modes_.end(), - [](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) && + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO); - })) { - RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity " - "interface running."); - ret_val = hardware_interface::return_type::ERROR; - } - - if (passthrough_trajectory_controller_running_ && - std::none_of(stop_modes_.begin(), stop_modes_.end(), - [](auto item) { return item == StoppingInterface::STOP_PASSTHROUGH; }) && - std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); })) { - RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while there is the passthrough " - "interface running."); - ret_val = hardware_interface::return_type::ERROR; - } - - // all start interfaces must be the same - can't mix position and velocity control - if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { - RCLCPP_ERROR(get_logger(), "Can't mix different control domains for joint control."); - ret_val = hardware_interface::return_type::ERROR; - } - - // stop all interfaces at the same time - if (stop_modes_.size() != 0 && - (stop_modes_.size() != 6 || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) { - RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be stopped at once."); ret_val = hardware_interface::return_type::ERROR; } diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py new file mode 100644 index 000000000..cecfe3e2c --- /dev/null +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -0,0 +1,289 @@ +#!/usr/bin/env python +# Copyright 2019, Universal Robots A/S +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import logging +import os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, + ROBOT_JOINTS, +) + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + def test_activating_multiple_controllers_same_interface_fails(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller" + ], + ).ok + ) + + # Activating different motion controllers should not be possible + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + ).ok + ) + + def test_activating_multiple_controllers_different_interface_fails(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller" + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + "forward_velocity_controller", + ], + ).ok + ) + + def test_activating_controller_with_running_position_controller_fails(self): + # Having a position-based controller active, no other controller should be able to + # activate. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + ], + deactivate_controllers=[ + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller" + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) + # Stop controller again + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + ], + ).ok + ) + + def test_activating_controller_with_running_passthrough_trajectory_controller_fails(self): + # Having a position-based controller active, no other controller should be able to + # activate. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller" + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "joint_trajectory_controller", + ], + ).ok + ) + # Stop the controller again + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index f21b4cacc..a8a1a8ffb 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -63,6 +63,15 @@ TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. TIMEOUT_WAIT_ACTION = 10 +ROBOT_JOINTS = [ + "elbow_joint", + "shoulder_lift_joint", + "shoulder_pan_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + def _wait_for_service(node, srv_name, srv_type, timeout): client = node.create_client(srv_type, srv_name)