diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 79a89acbd..4d172d22b 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -51,6 +51,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS include_directories(include) +generate_parameter_library( + tool_contact_controller_parameters + src/tool_contact_controller_parameters.yaml +) + generate_parameter_library( force_mode_controller_parameters src/force_mode_controller_parameters.yaml @@ -87,6 +92,7 @@ generate_parameter_library( ) add_library(${PROJECT_NAME} SHARED + src/tool_contact_controller.cpp src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp @@ -99,6 +105,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE include ) target_link_libraries(${PROJECT_NAME} + tool_contact_controller_parameters force_mode_controller_parameters gpio_controller_parameters speed_scaling_state_broadcaster_parameters @@ -107,6 +114,7 @@ target_link_libraries(${PROJECT_NAME} passthrough_trajectory_controller_parameters ur_configuration_controller_parameters ) + ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index ec15809a1..f4c916aa3 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -34,4 +34,9 @@ Controller used to get and change the configuration of the robot + + + Controller to use the tool contact functionality of the robot. + + diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index b33350889..06ae93491 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -11,6 +11,7 @@ robot family. Currently this contains: but it uses the speed scaling reported to align progress of the trajectory between the robot and controller. * A **io_and_status_controller** that allows setting I/O ports, controlling some UR-specific functionality and publishes status information about the robot. +* A **tool_contact_controller** that exposes an action to enable the tool contact function on the robot. About this package ------------------ @@ -378,3 +379,46 @@ The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msg * to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the controller timeout. + +.. _tool_contact_controller: + +ur_controllers/ToolContactController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This controller can enable tool contact on the robot. When tool contact is enabled, and the robot +senses whether the tool has made contact with something. When that happens, it will stop all +motion, and retract to where it first sensed the contact. + +This controller can be used with any of the motion controllers. + +The controller is not a direct representation of the URScript function `tool_contact(direction) +`_, +as it does not allow for choosing the direction. The direction of tool contact will always be the +current TCP direction of movement. + +Parameters +"""""""""" + ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``action_monitor_rate`` | double | 20.0 | The rate at which the action should be monitored in Hz. | ++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+ + +Action interface / usage +"""""""""""""""""""""""" +The controller provides one action for enabling tool contact. For the controller to accept action goals it needs to be in ``active`` state. + +* ``~/detect_tool_contact [ur_msgs/action/ToolContact]`` + + The action definition of ``ur_msgs/action/ToolContact`` has no fields, as a call to the action implicitly means that tool contact should be enabled. + The result of the action is available through the status of the action itself. If the action succeeds it means that tool contact was detected, otherwise tool contact will remain active until it is either cancelled by the user, or aborted by the hardware. + The action provides no feedback. + + The action can be called from the command line using the following command, when the controller is active: + + .. code-block:: + + ros2 action send_goal /tool_contact_controller/detect_tool_contact ur_msgs/action/ToolContact diff --git a/ur_controllers/include/ur_controllers/tool_contact_controller.hpp b/ur_controllers/include/ur_controllers/tool_contact_controller.hpp new file mode 100644 index 000000000..809628e13 --- /dev/null +++ b/ur_controllers/include/ur_controllers/tool_contact_controller.hpp @@ -0,0 +1,135 @@ +// Copyright 2025, 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. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Jacob Larsen jala@universal-robots.com + * \date 2025-01-07 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_ +#define UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_ + +#include +#include +#include + +#include +#include "std_msgs/msg/bool.hpp" +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "ur_controllers/tool_contact_controller_parameters.hpp" + +namespace ur_controllers +{ +class ToolContactController : public controller_interface::ControllerInterface +{ +public: + ToolContactController() = default; + ~ToolContactController() override = default; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + +private: + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + ur_msgs::action::ToolContact::Feedback::SharedPtr feedback_; ///< preallocated feedback + rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal + + // non-rt function that will be called with action_monitor_period to monitor the rt action + rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05); + void action_handler(); + + rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal); + + void + goal_accepted_callback(std::shared_ptr> goal_handle); + + rclcpp_action::CancelResponse goal_canceled_callback( + const std::shared_ptr> goal_handle); + + std::atomic tool_contact_enable_ = false; + std::atomic tool_contact_active_ = false; + std::atomic tool_contact_abort_ = false; + std::atomic change_requested_ = false; + std::atomic logged_once_ = false; + std::atomic should_reset_goal = false; + + std::optional> tool_contact_result_interface_; + std::optional> major_version_state_interface_; + std::optional> tool_contact_state_interface_; + std::optional> tool_contact_set_state_interface_; + + rclcpp_action::Server::SharedPtr tool_contact_action_server_; + + std::shared_ptr tool_contact_param_listener_; + tool_contact_controller::Params tool_contact_params_; + + static constexpr double TOOL_CONTACT_STANDBY = 1.0; + static constexpr double TOOL_CONTACT_WAITING_BEGIN = 2.0; + static constexpr double TOOL_CONTACT_EXECUTING = 3.0; + static constexpr double TOOL_CONTACT_FAILURE_BEGIN = 4.0; + static constexpr double TOOL_CONTACT_WAITING_END = 5.0; + static constexpr double TOOL_CONTACT_SUCCESS_END = 6.0; + static constexpr double TOOL_CONTACT_FAILURE_END = 7.0; +}; +} // namespace ur_controllers + +#endif // UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_ diff --git a/ur_controllers/src/tool_contact_controller.cpp b/ur_controllers/src/tool_contact_controller.cpp new file mode 100644 index 000000000..efa9b76d1 --- /dev/null +++ b/ur_controllers/src/tool_contact_controller.cpp @@ -0,0 +1,345 @@ +// Copyright 2025, 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. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Jacob Larsen jala@universal-robots.com + * \date 2025-01-07 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include +#include +#include +#include + +namespace ur_controllers +{ + +controller_interface::CallbackReturn ToolContactController::on_init() +{ + tool_contact_param_listener_ = std::make_shared(get_node()); + tool_contact_params_ = tool_contact_param_listener_->get_params(); + feedback_ = std::make_shared(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ToolContactController::on_configure(const rclcpp_lifecycle::State& /* previous_state */) +{ + tool_contact_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/detect_tool_contact", + std::bind(&ToolContactController::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ToolContactController::goal_canceled_callback, this, std::placeholders::_1), + std::bind(&ToolContactController::goal_accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration ToolContactController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + const std::string tf_prefix = tool_contact_params_.tf_prefix; + + config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_set_state"); + return config; +} + +controller_interface::InterfaceConfiguration ToolContactController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = tool_contact_params_.tf_prefix; + config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_result"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major"); + config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_state"); + return config; +} + +controller_interface::CallbackReturn +ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_state */) +{ + { + const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_state"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + tool_contact_state_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + { + const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_set_state"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + tool_contact_set_state_interface_ = *it; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + { + const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_result"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + tool_contact_result_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + { + const std::string interface_name = tool_contact_params_.tf_prefix + "get_robot_software_version/get_version_major"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + major_version_state_interface_ = *it; + double major_version = major_version_state_interface_->get().get_value(); + if (major_version < 5) { + RCLCPP_ERROR(get_node()->get_logger(), "This feature is not supported on CB3 robots, controller will not be " + "started."); + return controller_interface::CallbackReturn::ERROR; + } + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + action_monitor_period_ = rclcpp::Duration(rclcpp::Rate(tool_contact_params_.action_monitor_rate).period()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ToolContactController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */) +{ + // Abort active goal (if any) + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) { + RCLCPP_INFO(get_node()->get_logger(), "Aborting tool contact, as controller has been deactivated."); + // Mark the current goal as abort + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + if (tool_contact_active_) { + tool_contact_active_ = false; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ToolContactController::on_shutdown(const rclcpp_lifecycle::State& + /* previous_state */) +{ + tool_contact_action_server_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} + +rclcpp_action::GoalResponse ToolContactController::goal_received_callback( + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr /* goal */) +{ + RCLCPP_INFO(get_node()->get_logger(), "New goal received."); + + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact controller is not in active state, can not accept action " + "goals."); + return rclcpp_action::GoalResponse::REJECT; + } + + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact already active, rejecting goal."); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void ToolContactController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Goal accepted."); + tool_contact_enable_ = true; + tool_contact_abort_ = false; + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + goal_handle_timer_.reset(); + auto period = std::chrono::duration_cast( + std::chrono::nanoseconds(action_monitor_period_.nanoseconds())); + goal_handle_timer_ = get_node()->create_wall_timer(period, std::bind(&ToolContactController::action_handler, this)); + return; +} + +void ToolContactController::action_handler() +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { + // Allow the goal to handle any actions it needs to perform + active_goal->runNonRealtime(); + // If one of the goal ending conditions were met, reset our active goal pointer + if (should_reset_goal) { + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + should_reset_goal = false; + } + } +} + +rclcpp_action::CancelResponse ToolContactController::goal_canceled_callback( + const std::shared_ptr> goal_handle) +{ + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { + RCLCPP_INFO(get_node()->get_logger(), "Cancel tool contact requested."); + + // Mark the current goal as canceled + auto result = std::make_shared(); + active_goal->setCanceled(result); + should_reset_goal = true; + tool_contact_abort_ = true; + tool_contact_enable_ = false; + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +controller_interface::return_type ToolContactController::update(const rclcpp::Time& /* time */, + const rclcpp::Duration& /* period */) +{ + // Abort takes priority + if (tool_contact_abort_) { + tool_contact_abort_ = false; + tool_contact_enable_ = false; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END); + } else if (tool_contact_enable_) { + tool_contact_enable_ = false; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_BEGIN); + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + const int state = static_cast(tool_contact_state_interface_->get().get_value()); + + switch (state) { + case static_cast(TOOL_CONTACT_EXECUTING): + { + tool_contact_active_ = true; + if (!logged_once_) { + RCLCPP_INFO(get_node()->get_logger(), "Tool contact enabled successfully."); + logged_once_ = true; + } + double result = tool_contact_result_interface_->get().get_value(); + if (result == 0.0) { + tool_contact_active_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Tool contact finished successfully."); + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END); + if (active_goal) { + auto result = std::make_shared(); + active_goal->setSucceeded(result); + should_reset_goal = true; + } + } else if (result == 1.0) { + tool_contact_active_ = false; + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact aborted by hardware."); + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + if (active_goal) { + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + } + } break; + + case static_cast(TOOL_CONTACT_FAILURE_BEGIN): + { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact could not be enabled."); + tool_contact_active_ = false; + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + + if (active_goal) { + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + } break; + + case static_cast(TOOL_CONTACT_SUCCESS_END): + { + if (tool_contact_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Tool contact disabled successfully."); + tool_contact_active_ = false; + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + } + } break; + + case static_cast(TOOL_CONTACT_FAILURE_END): + { + RCLCPP_ERROR(get_node()->get_logger(), "Tool contact could not be disabled."); + + tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY); + + if (active_goal) { + auto result = std::make_shared(); + active_goal->setAborted(result); + should_reset_goal = true; + } + } break; + case static_cast(TOOL_CONTACT_STANDBY): + logged_once_ = false; + break; + default: + break; + } + if (active_goal) { + active_goal->setFeedback(feedback_); + } + + return controller_interface::return_type::OK; +} + +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::ToolContactController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/tool_contact_controller_parameters.yaml b/ur_controllers/src/tool_contact_controller_parameters.yaml new file mode 100644 index 000000000..9640915a6 --- /dev/null +++ b/ur_controllers/src/tool_contact_controller_parameters.yaml @@ -0,0 +1,11 @@ +tool_contact_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "The rate at which the action should be monitored in Hz." + } diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index baba5ffbd..adb8f7119 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -222,5 +222,9 @@ if(BUILD_TESTING) TIMEOUT 500 ) + add_launch_test(test/integration_test_tool_contact.py + TIMEOUT + 800 + ) endif() endif() diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 5e996bd24..333069294 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -42,6 +42,9 @@ controller_manager: ur_configuration_controller: type: ur_controllers/URConfigurationController + tool_contact_controller: + type: ur_controllers/ToolContactController + speed_scaling_state_broadcaster: ros__parameters: state_publish_rate: 100.0 @@ -175,3 +178,7 @@ tcp_pose_broadcaster: pose_name: $(var tf_prefix)tcp_pose tf: child_frame_id: $(var tf_prefix)tool0_controller + +tool_contact_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 87e2fd59f..3ff25ebeb 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -80,6 +80,7 @@ enum StoppingInterface STOP_PASSTHROUGH, STOP_FORCE_MODE, STOP_FREEDRIVE, + STOP_TOOL_CONTACT, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -168,6 +169,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void check_passthrough_trajectory_controller(); void trajectory_done_callback(urcl::control::TrajectoryResult result); bool is_valid_joint_information(std::vector> data); + void tool_contact_callback(urcl::control::ToolContactResult); + void check_tool_contact_controller(); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -229,6 +232,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Tool contact controller interface values + double tool_contact_set_state_; + double tool_contact_state_; + double tool_contact_result_; + bool tool_contact_controller_running_; + // Freedrive mode controller interface values bool freedrive_activated_; bool freedrive_mode_controller_running_; @@ -306,6 +315,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; const std::string FORCE_MODE_GPIO = "force_mode"; const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; + const std::string TOOL_CONTACT_GPIO = "tool_contact"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 10d59b017..d8f2af710 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -362,6 +362,7 @@ def controller_spawner(controllers, active=True): "force_mode_controller", "passthrough_trajectory_controller", "freedrive_mode_controller", + "tool_contact_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index dc9f28ec9..1719016cc 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -83,6 +83,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys velocity_controller_running_ = false; freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; + tool_contact_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -95,6 +96,9 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys passthrough_trajectory_transfer_state_ = 0.0; passthrough_trajectory_abort_ = 0.0; passthrough_trajectory_size_ = 0.0; + tool_contact_result_ = NO_NEW_CMD_; + tool_contact_set_state_ = 0.0; + tool_contact_state_ = 0.0; trajectory_joint_positions_.reserve(32768); trajectory_joint_velocities_.reserve(32768); trajectory_joint_accelerations_.reserve(32768); @@ -267,6 +271,12 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back(hardware_interface::StateInterface( tf_prefix + "get_robot_software_version", "get_version_build", &get_robot_software_version_build_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_result", &tool_contact_result_)); + + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_state", &tool_contact_state_)); + return state_interfaces; } @@ -404,6 +414,9 @@ std::vector URPositionHardwareInterface::e &passthrough_trajectory_accelerations_[i])); } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_set_state", &tool_contact_set_state_)); + return command_interfaces; } @@ -587,6 +600,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou ur_driver_->registerTrajectoryDoneCallback( std::bind(&URPositionHardwareInterface::trajectory_done_callback, this, std::placeholders::_1)); + ur_driver_->registerToolContactResultCallback( + std::bind(&URPositionHardwareInterface::tool_contact_callback, this, std::placeholders::_1)); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -805,6 +821,10 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: stop_force_mode(); } + if (tool_contact_controller_running_) { + check_tool_contact_controller(); + } + packet_read_ = false; } @@ -907,7 +927,7 @@ void URPositionHardwareInterface::checkAsyncIO() } if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { - RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Starting freedrive mode."); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Starting freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); freedrive_mode_enable_ = NO_NEW_CMD_; @@ -916,7 +936,7 @@ void URPositionHardwareInterface::checkAsyncIO() if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_activated_ && ur_driver_ != nullptr) { - RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Stopping freedrive mode."); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); freedrive_activated_ = false; @@ -924,6 +944,44 @@ void URPositionHardwareInterface::checkAsyncIO() } } +void URPositionHardwareInterface::check_tool_contact_controller() +{ + static double cmd_state; + cmd_state = tool_contact_set_state_; + + if (ur_driver_ != nullptr) { + if (cmd_state == 2.0) { + bool success = ur_driver_->startToolContact(); + if (success) { + // TOOL_CONTACT_EXECUTING + tool_contact_state_ = 3.0; + tool_contact_result_ = 3.0; + } else { + // TOOL_CONTACT_FAILURE_BEGIN + tool_contact_state_ = 4.0; + } + + } else if (cmd_state == 5.0) { + bool success = ur_driver_->endToolContact(); + if (success) { + // TOOL_CONTACT_SUCCESS_END + tool_contact_state_ = 6.0; + } else { + // TOOL_CONTACT_FAILURE_END + tool_contact_state_ = 7.0; + } + } else { + tool_contact_state_ = cmd_state; + } + } +} + +void URPositionHardwareInterface::tool_contact_callback(urcl::control::ToolContactResult result) +{ + tool_contact_result_ = static_cast(result); + return; +} + void URPositionHardwareInterface::updateNonDoubleValues() { for (size_t i = 0; i < 18; ++i) { @@ -1031,12 +1089,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (freedrive_mode_controller_running_) { control_modes[i].push_back(FREEDRIVE_MODE_GPIO); } - } - - if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), - [&](const std::vector& other) { return other == start_modes_[0]; })) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); - return hardware_interface::return_type::ERROR; + if (tool_contact_controller_running_) { + control_modes[i].push_back(TOOL_CONTACT_GPIO); + } } // Starting interfaces @@ -1045,21 +1100,27 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod 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) { - if (!start_modes_[i].empty()) { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; + })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while " "there is another control mode already " "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 (!start_modes_[i].empty()) { + 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 || + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; + })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while " "there is another control mode already " "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; @@ -1090,10 +1151,26 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i].push_back(FREEDRIVE_MODE_GPIO); + } else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO; + })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller " + "while either the force mode or " + "freedrive controller is running."); + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(TOOL_CONTACT_GPIO); } } } + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), + [&](const std::vector& other) { return other == start_modes_[0]; })) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "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) { @@ -1130,6 +1207,12 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod [&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }), control_modes[i].end()); } + if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") { + stop_modes_[i].push_back(StoppingInterface::STOP_TOOL_CONTACT); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == TOOL_CONTACT_GPIO; }), + control_modes[i].end()); + } } } @@ -1146,9 +1229,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start passthrough_trajectory " - "control while there is either position or " - "velocity or freedrive mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory " + "control while there is either position or " + "velocity or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1158,15 +1241,15 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FREEDRIVE_MODE_GPIO); + item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1176,15 +1259,28 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // Tool contact controller requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == TOOL_CONTACT_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), + [this](auto& item) { return (item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start tool contact controller while " + "either the force mode controller or " + "the freedrive controller is running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1200,10 +1296,10 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start position control while there " - "is either trajectory passthrough or " - "velocity mode or force_mode or freedrive mode " - "running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there " + "is either trajectory passthrough or " + "velocity mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1219,10 +1315,10 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start velocity control while there " - "is either trajectory passthrough or " - "position mode or force_mode or freedrive mode " - "running."); + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velocity control while there " + "is either trajectory passthrough or " + "position mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1239,27 +1335,38 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod StoppingInterface::STOP_POSITION) != stop_modes_[0].end()) { position_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; - } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) { + } + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { + } + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { force_mode_controller_running_ = false; stop_force_mode(); - } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) { + } + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) { + RCLCPP_WARN(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping passthrough trajectory controller."); passthrough_trajectory_controller_running_ = false; passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); - } else if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), - StoppingInterface::STOP_FREEDRIVE) != stop_modes_[0].end()) { + } + if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FREEDRIVE) != stop_modes_[0].end()) { freedrive_mode_controller_running_ = false; freedrive_activated_ = false; freedrive_mode_abort_ = 1.0; } + if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) { + tool_contact_controller_running_ = false; + tool_contact_result_ = 3.0; + ur_driver_->endToolContact(); + } if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { @@ -1274,23 +1381,29 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; - } else if (start_modes_[0].size() != 0 && - std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { + } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { force_mode_controller_running_ = true; - } else if (start_modes_[0].size() != 0 && - std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { + } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; - } else if (start_modes_[0].size() != 0 && - std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { + } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; freedrive_mode_controller_running_ = true; freedrive_activated_ = false; } - + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), TOOL_CONTACT_GPIO) != start_modes_[0].end()) { + tool_contact_controller_running_ = true; + } start_modes_.clear(); stop_modes_.clear(); diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 1d2d6570e..3c8a31a26 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -405,3 +405,92 @@ def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self): ], ).ok ) + + def test_tool_contact_compatibility(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", + "force_mode_controller", + "tool_contact_controller", + ], + ).ok + ) + + time.sleep(3) + # Start tool contact controller and JTC + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + + # Start tool contact controller and passthrough trajectory + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "passthrough_trajectory_controller", + "tool_contact_controller", + ], + ).ok + ) + + # tool contact should not start with force_mode + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "force_mode_controller", + "tool_contact_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "tool_contact_controller", + "force_mode_controller", + ], + ).ok + ) + + # tool contact should not start with freedrive + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "tool_contact_controller", + "freedrive_mode_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/integration_test_tool_contact.py b/ur_robot_driver/test/integration_test_tool_contact.py new file mode 100644 index 000000000..383d362b0 --- /dev/null +++ b/ur_robot_driver/test/integration_test_tool_contact.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python +# Copyright 2025, 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 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 +from ur_msgs.action import ToolContact +from action_msgs.msg import GoalStatus + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + ActionInterface, + generate_driver_test_description, +) + + +@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) + self._tool_contact_interface = ActionInterface( + self.node, "/tool_contact_controller/detect_tool_contact", ToolContact + ) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Tests + # + + def test_start_tool_contact_controller(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + + def test_goal_can_be_canceled(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + goal_handle = self._tool_contact_interface.send_goal() + self.assertTrue(goal_handle.accepted) + + cancel_res = self._tool_contact_interface.cancel_goal(goal_handle) + self.assertEqual(cancel_res.return_code, 0) + + def test_deactivate_controller_aborts_action(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertTrue(goal_handle.accepted) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["tool_contact_controller"], + ).ok + ) + # Wait for action to finish + self._tool_contact_interface.get_result(goal_handle, 5) + # Check status of goal handle, as result does not contain information about the status of the action. Only the empty result definition. + self.assertEqual(goal_handle._status, GoalStatus.STATUS_ABORTED) + + def test_inactive_controller_rejects_actions(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["tool_contact_controller"], + ).ok + ) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertFalse(goal_handle.accepted) + + def test_busy_controller_rejects_actions(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["tool_contact_controller"], + ).ok + ) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertTrue(goal_handle.accepted) + + goal_handle = self._tool_contact_interface.send_goal() + self.assertFalse(goal_handle.accepted) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 018ec74d2..640a7afaf 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -191,6 +191,18 @@ def get_result(self, goal_handle, timeout): f"Exception while calling action '{self.__action_name}': {future_res.exception()}" ) + def cancel_goal(self, goal_handle, timeout=2): + future_res = goal_handle.cancel_goal_async() + logging.info("Canceling goal from '%s' with timeout %fs", self.__action_name, timeout) + rclpy.spin_until_future_complete(self.__node, future_res, timeout_sec=timeout) + if future_res.result() is not None: + logging.info(" Received result: %s", future_res.result()) + return future_res.result() + else: + raise Exception( + f"Exception while calling action '{self.__action_name}': {future_res.exception()}" + ) + class DashboardInterface( _ServiceInterface, @@ -353,7 +365,9 @@ def generate_dashboard_test_description(): def generate_driver_test_description( - tf_prefix="", controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL + tf_prefix="", + initial_joint_controller="scaled_joint_trajectory_controller", + controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL, ): ur_type = LaunchConfiguration("ur_type") @@ -362,7 +376,7 @@ def generate_driver_test_description( "ur_type": ur_type, "launch_rviz": "false", "controller_spawner_timeout": str(controller_spawner_timeout), - "initial_joint_controller": "scaled_joint_trajectory_controller", + "initial_joint_controller": initial_joint_controller, "headless_mode": "true", "launch_dashboard_client": "true", "start_joint_controller": "false",