diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index f310a32e8..79a89acbd 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -71,6 +71,11 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + freedrive_mode_controller_parameters + src/freedrive_mode_controller_parameters.yaml +) + generate_parameter_library( passthrough_trajectory_controller_parameters src/passthrough_trajectory_controller_parameters.yaml @@ -85,6 +90,7 @@ add_library(${PROJECT_NAME} SHARED src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp + src/freedrive_mode_controller.cpp src/gpio_controller.cpp src/passthrough_trajectory_controller.cpp src/ur_configuration_controller.cpp) @@ -97,6 +103,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + freedrive_mode_controller_parameters passthrough_trajectory_controller_parameters ur_configuration_controller_parameters ) @@ -153,6 +160,16 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + ament_add_gmock(test_load_freedrive_mode_controller + test/test_load_freedrive_mode_controller.cpp + ) + target_link_libraries(test_load_freedrive_mode_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_load_freedrive_mode_controller + controller_manager + ros2_control_test_assets + ) endif() ament_package() diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f3365a26d..ec15809a1 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -19,6 +19,11 @@ Controller to use UR's force_mode. + + + This controller handles the activation of the freedrive mode. + + This controller forwards a joint-based trajectory to the robot controller for interpolation. diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 38211d083..f03632d53 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -340,3 +340,41 @@ damping_factor gain_scaling Force mode gain scaling factor. Scales the gain in force mode. scaling parameter is in range [0;2], default is 0.5. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces. + +.. _freedrive_mode_controller: + +ur_controllers/FreedriveModeController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller activates the robot's *Freedrive Mode*, allowing to manually move the robot' joints. +This controller can't be combined with any other motion controller. + +Parameters +"""""""""" + ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``inactive_timeout`` | int | 1 | Time interval (in seconds) of message inactivity after which freedrive is deactivated | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ + +Usage +""""" + +The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: + +* to start and keep freedrive active, you'll have to frequently publish a ``True`` msg on the indicated topic. + If no further messages are received by the controller within the ``inactive_timeout`` seconds, + freedrive mode will be deactivated. Hence, it is recommended to publish a ``True`` message at least every + ``inactive_timeout/2`` seconds. + + .. code-block:: + + ros2 topic pub --rate 2 /freedrive_mode_controller/enable_freedrive_mode std_msgs/msg/Bool "{data: true}" + +* 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. diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp new file mode 100644 index 000000000..04e90805e --- /dev/null +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -0,0 +1,130 @@ +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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 Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-26 + */ +//---------------------------------------------------------------------- +#ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ +#define UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/bool.hpp" + +#include "freedrive_mode_controller_parameters.hpp" + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FREEDRIVE_MODE_ASYNC_SUCCESS = 0u, + FREEDRIVE_MODE_ENABLE = 1, + FREEDRIVE_MODE_ABORT = 2, +}; + +using namespace std::chrono_literals; // NOLINT + +class FreedriveModeController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + // Change the input for the update function + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + // Command interfaces: optional is used only to avoid adding reference initialization + std::optional> async_success_command_interface_; + std::optional> enable_command_interface_; + std::optional> abort_command_interface_; + + std::shared_ptr> enable_freedrive_mode_sub_; + + rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check for timeout on input + mutable std::chrono::seconds timeout_interval_; + void freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg); + + std::shared_ptr freedrive_param_listener_; + freedrive_mode_controller::Params freedrive_params_; + + std::atomic freedrive_active_; + std::atomic change_requested_; + std::atomic async_state_; + std::atomic first_log_; + std::atomic timer_started_; + + void start_timer(); + void timeout_callback(); + + std::thread logging_thread_; + std::atomic logging_thread_running_; + std::atomic logging_requested_; + std::condition_variable logging_condition_; + std::mutex log_mutex_; + void log_task(); + void start_logging_thread(); + void stop_logging_thread(); + + static constexpr double ASYNC_WAITING = 2.0; + /** + * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries + * have been reached + */ + bool waitForAsyncCommand(std::function get_value); +}; +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp new file mode 100644 index 000000000..e1ae016e5 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -0,0 +1,337 @@ + +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of 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 Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-16 + */ +//---------------------------------------------------------------------- +#include +#include +#include +#include +#include + +namespace ur_controllers +{ +controller_interface::CallbackReturn FreedriveModeController::on_init() +{ + try { + // Create the parameter listener and get the parameters + freedrive_param_listener_ = std::make_shared(get_node()); + freedrive_params_ = freedrive_param_listener_->get_params(); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration FreedriveModeController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = freedrive_params_.tf_prefix; + timeout_interval_ = std::chrono::seconds(freedrive_params_.inactive_timeout); + + // Get the command interfaces needed for freedrive mode from the hardware interface + config.names.emplace_back(tf_prefix + "freedrive_mode/async_success"); + config.names.emplace_back(tf_prefix + "freedrive_mode/enable"); + config.names.emplace_back(tf_prefix + "freedrive_mode/abort"); + + return config; +} + +controller_interface::InterfaceConfiguration +ur_controllers::FreedriveModeController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; + + return config; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + // Subscriber definition + enable_freedrive_mode_sub_ = get_node()->create_subscription( + "~/enable_freedrive_mode", 10, + std::bind(&FreedriveModeController::freedrive_cmd_callback, this, std::placeholders::_1)); + + timer_started_ = false; + + const auto logger = get_node()->get_logger(); + + if (!freedrive_param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); + return controller_interface::CallbackReturn::ERROR; + } + + // Update the dynamic map parameters + freedrive_param_listener_->refresh_dynamic_parameters(); + + // Get parameters from the listener in case they were updated + freedrive_params_ = freedrive_param_listener_->get_params(); + + start_logging_thread(); + + return ControllerInterface::on_configure(previous_state); +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& state) +{ + change_requested_ = false; + freedrive_active_ = false; + async_state_ = std::numeric_limits::quiet_NaN(); + + first_log_ = false; + logging_thread_running_ = true; + logging_requested_ = false; + + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "async_success"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + async_success_command_interface_ = *it; + } 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 = freedrive_params_.tf_prefix + "freedrive_mode/" + "enable"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + enable_command_interface_ = *it; + } 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 = freedrive_params_.tf_prefix + "freedrive_mode/" + "abort"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + abort_command_interface_ = *it; + abort_command_interface_->get().set_value(0.0); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + return ControllerInterface::on_activate(state); +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) +{ + abort_command_interface_->get().set_value(1.0); + + stop_logging_thread(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) +{ + freedrive_active_ = false; + + freedrive_sub_timer_.reset(); + timer_started_ = false; + + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + async_state_ = async_success_command_interface_->get().get_value(); + + if (change_requested_) { + if (freedrive_active_) { + // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the + // teach pendant. + if (!std::isnan(abort_command_interface_->get().get_value()) && + abort_command_interface_->get().get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request."); + freedrive_active_ = false; + return controller_interface::return_type::OK; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); + + // Set command interface to enable + enable_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + + } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); + + abort_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + first_log_ = true; + change_requested_ = false; + } + + if ((async_state_ == 1.0) && (first_log_)) { + first_log_ = false; + logging_requested_ = true; + + // Notify logging thread + logging_condition_.notify_one(); + } + return controller_interface::return_type::OK; +} + +void FreedriveModeController::freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg) +{ + // Process the freedrive_mode command. + if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (msg->data) { + if ((!freedrive_active_) && (!change_requested_)) { + freedrive_active_ = true; + change_requested_ = true; + start_timer(); + } + } else { + if ((freedrive_active_) && (!change_requested_)) { + freedrive_active_ = false; + change_requested_ = true; + } + } + } + + if (freedrive_sub_timer_) { + freedrive_sub_timer_->reset(); + } +} + +void FreedriveModeController::start_timer() +{ + if (!timer_started_) { + // Start the timer only after the first message is received + freedrive_sub_timer_ = + get_node()->create_wall_timer(timeout_interval_, std::bind(&FreedriveModeController::timeout_callback, this)); + timer_started_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); + } +} + +void FreedriveModeController::timeout_callback() +{ + if (timer_started_ && freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since no new message received."); + + freedrive_active_ = false; + change_requested_ = true; + } + + timer_started_ = false; +} + +void FreedriveModeController::start_logging_thread() +{ + if (!logging_thread_running_) { + logging_thread_running_ = true; + logging_thread_ = std::thread(&FreedriveModeController::log_task, this); + } +} + +void FreedriveModeController::stop_logging_thread() +{ + logging_thread_running_ = false; + if (logging_thread_.joinable()) { + logging_thread_.join(); + } +} + +void FreedriveModeController::log_task() +{ + while (logging_thread_running_) { + std::unique_lock lock(log_mutex_); + + auto condition = [this] { return !logging_thread_running_ || logging_requested_; }; + + // Wait for the condition + logging_condition_.wait(lock, condition); + + if (!logging_thread_running_) + break; + + if (freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } + + // Reset to log only once + logging_requested_ = false; + } +} + +bool FreedriveModeController::waitForAsyncCommand(std::function get_value) +{ + const auto maximum_retries = freedrive_params_.check_io_successful_retries; + int retries = 0; + while (get_value() == ASYNC_WAITING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + retries++; + + if (retries > maximum_retries) + return false; + } + return true; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/freedrive_mode_controller_parameters.yaml b/ur_controllers/src/freedrive_mode_controller_parameters.yaml new file mode 100644 index 000000000..e50b42260 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -0,0 +1,17 @@ +--- +freedrive_mode_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + inactive_timeout: { + type: int, + default_value: 1, + description: "Time interval (in seconds) of message inactivity after which freedrive is deactivated" + } + check_io_successful_retries: { + type: int, + default_value: 10, + description: "Amount of retries for checking if setting force_mode was successful" + } diff --git a/ur_controllers/test/freedrive_mode_controller_params.yaml b/ur_controllers/test/freedrive_mode_controller_params.yaml new file mode 100644 index 000000000..417d385c0 --- /dev/null +++ b/ur_controllers/test/freedrive_mode_controller_params.yaml @@ -0,0 +1,5 @@ +--- +freedrive_mode_controller: + ros__parameters: + tf_prefix: "" + inactive_timeout: 10 diff --git a/ur_controllers/test/test_load_freedrive_mode_controller.cpp b/ur_controllers/test/test_load_freedrive_mode_controller.cpp new file mode 100644 index 000000000..cf3ce4292 --- /dev/null +++ b/ur_controllers/test/test_load_freedrive_mode_controller.cpp @@ -0,0 +1,60 @@ +// Copyright 2024, 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. + +#include +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadFreedriveModeController, load_controller) +{ + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm{ executor, ros2_control_test_assets::minimal_robot_urdf, true, + "test_controller_manager" }; + + const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/freedrive_mode_controller_params.yaml"; + cm.set_parameter({ "test_freedrive_mode_controller.params_file", test_file_path }); + + cm.set_parameter({ "test_freedrive_mode_controller.type", "ur_controllers/FreedriveModeController" }); + + ASSERT_NE(cm.load_controller("test_freedrive_mode_controller"), nullptr); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 232e081e1..3fc321c01 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -27,6 +27,9 @@ controller_manager: force_mode_controller: type: ur_controllers/ForceModeController + freedrive_mode_controller: + type: ur_controllers/FreedriveModeController + passthrough_trajectory_controller: type: ur_controllers/PassthroughTrajectoryController @@ -159,6 +162,10 @@ force_mode_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" +freedrive_mode_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + tcp_pose_broadcaster: ros__parameters: frame_id: $(var tf_prefix)base diff --git a/ur_robot_driver/doc/usage/controllers.rst b/ur_robot_driver/doc/usage/controllers.rst index a35a8b084..67c3a4926 100644 --- a/ur_robot_driver/doc/usage/controllers.rst +++ b/ur_robot_driver/doc/usage/controllers.rst @@ -117,3 +117,11 @@ force_mode_controller Type: :ref:`ur_controllers/ForceModeController ` Allows utilizing the robot's builtin *Force Mode*. + +freedrive_mode_controller +^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/FreedriveModeController ` + +Allows utilizing the robot's *Freedrive mode*, making it possible to manually move the robot's +joints. 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 1f665a1b2..4d525e92a 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -79,7 +79,8 @@ enum StoppingInterface STOP_POSITION, STOP_VELOCITY, STOP_PASSTHROUGH, - STOP_FORCE_MODE + STOP_FORCE_MODE, + STOP_FREEDRIVE, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -228,6 +229,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Freedrive mode controller interface values + bool freedrive_activated_; + bool freedrive_mode_controller_running_; + double freedrive_mode_async_success_; + double freedrive_mode_enable_; + double freedrive_mode_abort_; + // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; double passthrough_trajectory_abort_; @@ -236,6 +244,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t passthrough_trajectory_velocities_; urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; + // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -293,8 +302,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); - const std::string FORCE_MODE_GPIO = "force_mode"; const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; + const std::string FORCE_MODE_GPIO = "force_mode"; + const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 2bfde7780..da829645d 100644 --- a/ur_robot_driver/launch/ur10.launch.py +++ b/ur_robot_driver/launch/ur10.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur10e.launch.py b/ur_robot_driver/launch/ur10e.launch.py index 18c909225..366204c49 100644 --- a/ur_robot_driver/launch/ur10e.launch.py +++ b/ur_robot_driver/launch/ur10e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur16e.launch.py b/ur_robot_driver/launch/ur16e.launch.py index 82850e5ee..cb5390505 100644 --- a/ur_robot_driver/launch/ur16e.launch.py +++ b/ur_robot_driver/launch/ur16e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur20.launch.py b/ur_robot_driver/launch/ur20.launch.py index 547c9293d..1d1907235 100644 --- a/ur_robot_driver/launch/ur20.launch.py +++ b/ur_robot_driver/launch/ur20.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur3.launch.py b/ur_robot_driver/launch/ur3.launch.py index 38de8f5f4..ea9cc44db 100644 --- a/ur_robot_driver/launch/ur3.launch.py +++ b/ur_robot_driver/launch/ur3.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py index 95478caa1..89f373e2e 100644 --- a/ur_robot_driver/launch/ur30.launch.py +++ b/ur_robot_driver/launch/ur30.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur3e.launch.py b/ur_robot_driver/launch/ur3e.launch.py index 78c145a87..d84b6acc9 100644 --- a/ur_robot_driver/launch/ur3e.launch.py +++ b/ur_robot_driver/launch/ur3e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur5.launch.py b/ur_robot_driver/launch/ur5.launch.py index c70a6bc39..8c0be7aec 100644 --- a/ur_robot_driver/launch/ur5.launch.py +++ b/ur_robot_driver/launch/ur5.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index 433f5e701..97518ea4c 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 8804cc4eb..d0c5e1e4a 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -177,6 +177,7 @@ def controller_spawner(controllers, active=True): "forward_position_controller", "force_mode_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) @@ -322,6 +323,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], description="Initially loaded robot controller.", diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 6ec549291..216d7d462 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 urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; position_controller_running_ = false; velocity_controller_running_ = false; + freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; @@ -92,6 +93,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; + freedrive_mode_abort_ = 0.0; passthrough_trajectory_transfer_state_ = 0.0; passthrough_trajectory_abort_ = 0.0; trajectory_joint_positions_.clear(); @@ -365,6 +367,15 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "async_success", + &freedrive_mode_async_success_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "enable", &freedrive_mode_enable_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "abort", &freedrive_mode_abort_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", &passthrough_trajectory_transfer_state_)); @@ -716,6 +727,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; force_mode_disable_cmd_ = NO_NEW_CMD_; + freedrive_mode_abort_ = NO_NEW_CMD_; + freedrive_mode_enable_ = NO_NEW_CMD_; initialized_ = true; } @@ -744,6 +757,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); + } else if (freedrive_mode_controller_running_ && freedrive_activated_) { + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); + } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); check_passthrough_trajectory_controller(); @@ -859,6 +875,23 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); zero_ftsensor_cmd_ = NO_NEW_CMD_; } + + if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { + RCLCPP_INFO(get_logger(), "Starting freedrive mode."); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_enable_ = NO_NEW_CMD_; + freedrive_activated_ = true; + } + + if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_activated_ && + ur_driver_ != nullptr) { + RCLCPP_INFO(get_logger(), "Stopping freedrive mode."); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_activated_ = false; + freedrive_mode_abort_ = NO_NEW_CMD_; + } } void URPositionHardwareInterface::updateNonDoubleValues() @@ -943,6 +976,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (passthrough_trajectory_controller_running_) { control_modes[i].push_back(PASSTHROUGH_GPIO); } + if (freedrive_mode_controller_running_) { + control_modes[i].push_back(FREEDRIVE_MODE_GPIO); + } } if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), @@ -988,6 +1024,14 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i].push_back(PASSTHROUGH_GPIO); + } else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { + 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 || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO; + })) { + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(FREEDRIVE_MODE_GPIO); } } } @@ -1022,33 +1066,61 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }), control_modes[i].end()); } + if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { + stop_modes_[i].push_back(StoppingInterface::STOP_FREEDRIVE); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }), + control_modes[i].end()); + } } } // Do not start conflicting controllers + // Passthrough controller requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_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); + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); }))) { RCLCPP_ERROR(get_logger(), "Attempting to start passthrough_trajectory control while there is either position or " - "velocity mode is running."); + "velocity or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } + // Force mode requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == FORCE_MODE_GPIO); }) && (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_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); + }))) { + RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or " + "velocity mode running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // Freedrive mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && + (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); }) || 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 == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); }))) { RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or " "velocity mode running."); @@ -1061,14 +1133,14 @@ 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 == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_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 == FREEDRIVE_MODE_GPIO); }))) { RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is either trajectory passthrough or " - "velocity mode or force_mode running."); + "velocity mode or force_mode or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1078,14 +1150,14 @@ 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_POSITION || item == PASSTHROUGH_GPIO || - item == FORCE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_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 == FREEDRIVE_MODE_GPIO); }))) { RCLCPP_ERROR(get_logger(), "Attempting to start velosity control while there is either trajectory passthrough or " - "position mode or force_mode running."); + "position mode or force_mode or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1117,6 +1189,11 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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()) { + freedrive_mode_controller_running_ = false; + freedrive_activated_ = false; + freedrive_mode_abort_ = 1.0; } if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), @@ -1141,6 +1218,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + freedrive_mode_controller_running_ = true; + freedrive_activated_ = false; } start_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 bdc9a0eca..a9356ffc7 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -96,6 +96,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): "forward_velocity_controller", "passthrough_trajectory_controller", "force_mode_controller", + "freedrive_mode_controller", ], ).ok ) @@ -132,6 +133,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): "forward_velocity_controller", "force_mode_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ], ).ok ) @@ -180,6 +182,15 @@ def test_activating_multiple_controllers_different_interface_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "freedrive_mode_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 @@ -195,6 +206,7 @@ def test_activating_controller_with_running_position_controller_fails(self): "forward_position_controller", "forward_velocity_controller", "force_mode_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -215,6 +227,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) self.assertFalse( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.STRICT, @@ -289,6 +309,14 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) # Stop the controller again self.assertTrue( self._controller_manager_interface.switch_controller( diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 35b0a8cb9..91e73a888 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -225,6 +225,12 @@ + + + + + +