diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c47a8345..0d0487a0 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 03da69f7..ca05c45b 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 00000000..cecfe3e2 --- /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 f21b4cac..a8a1a8ff 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)