From e989cf48716d94085cd83c646534d0088aff1665 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 25 Sep 2024 10:42:37 +0000 Subject: [PATCH 01/26] Initial Draft --- ur_controllers/CMakeLists.txt | 3 + .../freedrive_mode_controller.hpp | 97 ++++++++ .../src/freedrive_mode_controller.cpp | 215 ++++++++++++++++++ 3 files changed, 315 insertions(+) create mode 100644 ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp create mode 100644 ur_controllers/src/freedrive_mode_controller.cpp diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..7c1825273 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) @@ -31,6 +32,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools std_msgs std_srvs + tf2_ros ur_dashboard_msgs ur_msgs generate_parameter_library @@ -57,6 +59,7 @@ generate_parameter_library( add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp + src/freedrive_mode_controller.cpp src/gpio_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE 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..5e7517d1c --- /dev/null +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -0,0 +1,97 @@ +// Copyright 2023, 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 Felix Exner exner@fzi.de + * \date 2023-06-29 + */ +//---------------------------------------------------------------------- + +#pragma once +#include +#include +#include + +#include +#include +#include +#include + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FREEDRIVE_MODE_ASYNC_SUCCESS = 25, + FREEDRIVE_MODE_CMD = 26, +}; +enum StateInterfaces +{ + INITIALIZED_FLAG = 0u, +}; + +class FreedriveModeController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + 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_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); + bool enableFreedriveMode(); + bool disableFreedriveMode(); + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + std::shared_ptr param_listener_; + freedrive_mode_controller::Params params_; + + std::atomic freedrive_mode_enable_; + std::shared_ptr freedrive_mode_sub_; + + 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 diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp new file mode 100644 index 000000000..37687aeea --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -0,0 +1,215 @@ + +// Copyright 2023, 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 +namespace ur_controllers +{ +controller_interface::CallbackReturn FreedriveModeController::on_init() +{ + // I shouldn't need this, the only param I use is tf_prefix + try { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = 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 = params_.tf_prefix; + RCLCPP_DEBUG(get_node()->get_logger(), "Configure UR freedrive_mode controller with tf_prefix: %s", tf_prefix.c_str()); + + // Get the command interfaces needed for freedrive mode from the hardware interface + config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_async_success"); + config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_cmd"); + + return config; +} + +controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + // Get the state interface indicating whether the hardware interface has been initialized + config.names.emplace_back(tf_prefix + "system_interface/initialized"); + + return config; +} + +controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + // Take enable and update it + if (freedrive_mode_enable_) + { + enableFreedriveMode(); + } + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +{ + const auto logger = get_node()->get_logger(); + + if (!param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); + return controller_interface::CallbackReturn::ERROR; + } + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) { + RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize..."); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Create the publisher that will receive the command to start the freedrive_mode + try { + enable_freedrive_mode_sub_ = get_node()->create_subscription( + "~/start_free_drive_mode", 10, + std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + // Stop freedrive mode if this controller is deactivated. + disableFreedriveMode(); + try { + set_freedrive_mode_srv_.reset(); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) +{ + // Process the freedrive_mode command. + if(msg->data) + { + freedrive_mode_enable_ = true; + RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); + } else{ + freedrive_mode_enable_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); + } +} + +bool FreedriveModeController::enableFreedriveMode() +{ + // reset success flag + command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + + // Shouldn't I have a command set to 1 start it? Like it happens for the disable + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); + const auto maximum_retries = params_.check_io_successful_retries; + int retries = 0; + while (command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + retries++; + + if (retries > maximum_retries) { + resp->success = false; + } + } + + resp->success = static_cast(command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value()); + + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been set successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); + return false; + } + + return true; +} + +bool FreedriveModeController::disableFreedriveMode() +{ + command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_CMD].set_value(1); + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); + while (command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the freedrive mode + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + bool success = static_cast(command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value()); + if (success) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not disable freedrive mode."); + return false; + } + return true; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) +find_package(cartesian_controllers REQUIRED) From c83203c7abf521ffaf37a20a8f7eae520cedbf2f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Thu, 10 Oct 2024 08:58:38 +0000 Subject: [PATCH 02/26] Basic version for the new action server implementation --- .../freedrive_mode_controller.hpp | 36 ++++++++++++++----- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 5e7517d1c..38a0bad31 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -33,6 +33,8 @@ * \date 2023-06-29 */ //---------------------------------------------------------------------- +#ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ +#define UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ #pragma once #include @@ -40,8 +42,6 @@ #include #include -#include -#include #include namespace ur_controllers @@ -63,6 +63,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface 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; @@ -74,16 +75,34 @@ class FreedriveModeController : public controller_interface::ControllerInterface CallbackReturn on_init() override; private: - void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); - bool enableFreedriveMode(); - bool disableFreedriveMode(); + //void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); + //bool enableFreedriveMode(); + //bool disableFreedriveMode(); - std::unique_ptr tf_buffer_; - std::unique_ptr tf_listener_; + // Everything related to the RT action server + using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; + 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. + rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal + realtime_tools::RealtimeBuffer> joint_trajectory_mapping_; + + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + // Not sure this is needed anymore, for tf_prefix there are other ways to handle + // std::unique_ptr tf_buffer_; + // std::unique_ptr tf_listener_; std::shared_ptr param_listener_; freedrive_mode_controller::Params params_; + /* Start an action server with an action called: /freedrive_mode_controller/start_freedrive_mode. */ + void start_action_server(void); + + void end_goal(); + std::atomic freedrive_mode_enable_; std::shared_ptr freedrive_mode_sub_; @@ -94,4 +113,5 @@ class FreedriveModeController : public controller_interface::ControllerInterface */ bool waitForAsyncCommand(std::function get_value); }; -} // namespace ur_controllers +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ From 69e44ffc7b75d362b3a6d72804ec3db3165e0419 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sat, 12 Oct 2024 17:41:36 +0000 Subject: [PATCH 03/26] Added state interfaces and related vars --- .../include/ur_controllers/freedrive_mode_controller.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 38a0bad31..6f11c57be 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -79,6 +79,14 @@ class FreedriveModeController : public controller_interface::ControllerInterface //bool enableFreedriveMode(); //bool disableFreedriveMode(); + // State interfaces + realtime_tools::RealtimeBuffer> joint_names_; + std::vector state_interface_types_; + + std::vector joint_state_interface_names_; + std::vector> joint_position_state_interface_; + std::vector> joint_velocity_state_interface_; + // Everything related to the RT action server using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; @@ -104,6 +112,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface void end_goal(); std::atomic freedrive_mode_enable_; + std::atomic freedrive_active_; std::shared_ptr freedrive_mode_sub_; static constexpr double ASYNC_WAITING = 2.0; From 34582eee75ba5df4e28f3c21bb8a2c89cfc173e1 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sat, 12 Oct 2024 17:44:30 +0000 Subject: [PATCH 04/26] Updated on_configure, on_activate, on_deactivate --- .../src/freedrive_mode_controller.cpp | 145 +++++++++++++----- 1 file changed, 108 insertions(+), 37 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 37687aeea..14970a61e 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -42,11 +42,11 @@ namespace ur_controllers { controller_interface::CallbackReturn FreedriveModeController::on_init() { - // I shouldn't need this, the only param I use is tf_prefix + // Even if the only param I use is tf_prefix, I still need it try { // Create the parameter listener and get the parameters - param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); + 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; @@ -59,12 +59,20 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - const std::string tf_prefix = params_.tf_prefix; - RCLCPP_DEBUG(get_node()->get_logger(), "Configure UR freedrive_mode controller with tf_prefix: %s", tf_prefix.c_str()); + const std::string tf_prefix = freedrive_params_.tf_prefix; + + auto joint_names = freedrive_params_.joints; + for (auto& joint_name : joint_names) { + config.names.emplace_back(joint_name + "/position"); //hardware_interface::HW_IF_POSITION + config.names.emplace_back(joint_name + "/velocity"); //hardware_interface::HW_IF_VELOCITY + } + + config.names.push_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_abort"); + config.names.emplace_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_transfer_state"); // Get the command interfaces needed for freedrive mode from the hardware interface - config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_async_success"); - config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_cmd"); + // config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_async_success"); + // config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_cmd"); return config; } @@ -74,9 +82,11 @@ controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeContro controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - const std::string tf_prefix = params_.tf_prefix; - // Get the state interface indicating whether the hardware interface has been initialized - config.names.emplace_back(tf_prefix + "system_interface/initialized"); + // I'm not sure I need these interfaces + std::copy(joint_state_interface_names_.cbegin(), joint_state_interface_names_.cend(), std::back_inserter(config.names)); + + // This doesn't exist for me, so I will comment it for now + //config.names.push_back(freedrive_params_.speed_scaling_interface_name); return config; } @@ -89,59 +99,120 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat { enableFreedriveMode(); } - + return controller_interface::return_type::OK; } controller_interface::CallbackReturn -ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) { + + start_action_server(); + freedrive_active_ = false; + const auto logger = get_node()->get_logger(); - if (!param_listener_) { + if (!freedrive_param_listener_) { RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); return controller_interface::CallbackReturn::ERROR; } // update the dynamic map parameters - param_listener_->refresh_dynamic_parameters(); + freedrive_param_listener_->refresh_dynamic_parameters(); // get parameters from the listener in case they were updated - params_ = param_listener_->get_params(); + freedrive_params_ = /freedrive_param_listener_->get_params(); - return LifecycleNodeInterface::CallbackReturn::SUCCESS; + // Joint interfaces handling + joint_state_interface_names_.clear(); + + joint_state_interface_names_.reserve(number_of_joints_ * state_interface_types_.size()); + + auto joint_names_internal = joint_names_.readFromRT(); + for (const auto& joint_name : *joint_names_internal) { + for (const auto& interface_type : state_interface_types_) { + joint_state_interface_names_.emplace_back(joint_name + "/" + interface_type); + } + } + return ControllerInterface::on_configure(previous_state); +} + +void FreedriveModeController::start_action_server(void) +{ + freedrive_mode_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/freedrive_mode", + std::bind(&FreedriveModeController::goal_received_callback, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&FreedriveModeController::goal_cancelled_callback, this, std::placeholders::_1), + std::bind(&FreedriveModeController::goal_accepted_callback, this, std::placeholders::_1)); + return; } controller_interface::CallbackReturn -ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& state) { - while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) { - RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize..."); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + // clear out vectors in case of restart + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + + for (auto& interface_name : joint_state_interface_names_) { + auto interface_it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (interface_it != state_interfaces_.end()) { + if (interface_it->get_interface_name() == "position") { + joint_position_state_interface_.emplace_back(*interface_it); + + } else if (interface_it->get_interface_name() == "velocity") { + joint_velocity_state_interface_.emplace_back(*interface_it); + } + } } - // Create the publisher that will receive the command to start the freedrive_mode - try { - enable_freedrive_mode_sub_ = get_node()->create_subscription( - "~/start_free_drive_mode", 10, - std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); - } catch (...) { - return LifecycleNodeInterface::CallbackReturn::ERROR; + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" + "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; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } } - return LifecycleNodeInterface::CallbackReturn::SUCCESS; + + // Not sure if I need this one, check it out + const std::string tf_prefix = freedrive_params_.tf_prefix; + { + const std::string interface_name = tf_prefix + "freedrive_mode_controller/freedrive_mode_transfer_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()) { + transfer_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; + } + } + + return ControllerInterface::on_activate(state); } controller_interface::CallbackReturn -ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) { - // Stop freedrive mode if this controller is deactivated. - disableFreedriveMode(); - try { - set_freedrive_mode_srv_.reset(); - } catch (...) { - return LifecycleNodeInterface::CallbackReturn::ERROR; + abort_command_interface_->get().set_value(1.0); + if (freedrive_active_) { + const auto active_goal = *rt_active_goal_.readFromRT(); + std::shared_ptr result = + std::make_shared(); + result->set__error_string("Deactivating freedrive mode, since the controller is being deactivated."); + active_goal->setAborted(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + end_goal(); } - return LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) @@ -165,7 +236,7 @@ bool FreedriveModeController::enableFreedriveMode() // Shouldn't I have a command set to 1 start it? Like it happens for the disable RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); - const auto maximum_retries = params_.check_io_successful_retries; + const auto maximum_retries = freedrive_params_.check_io_successful_retries; int retries = 0; while (command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); From 2b2ecdfd68e1856dec6648e1cc3c35286bb42ad0 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sat, 12 Oct 2024 18:19:43 +0000 Subject: [PATCH 05/26] Adapted update to action server usage --- .../src/freedrive_mode_controller.cpp | 33 ++++++++----------- 1 file changed, 13 insertions(+), 20 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 14970a61e..7b606795c 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -68,7 +68,6 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in } config.names.push_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_abort"); - config.names.emplace_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_transfer_state"); // Get the command interfaces needed for freedrive mode from the hardware interface // config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_async_success"); @@ -94,12 +93,20 @@ controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeContro controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - // Take enable and update it - if (freedrive_mode_enable_) - { - enableFreedriveMode(); + const auto active_goal = *rt_active_goal_.readFromRT(); + + if (active_goal && trajectory_active_) { + // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the teach + // pendant. + if (abort_command_interface_->get().get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + end_goal(); + return controller_interface::return_type::OK; + } } - return controller_interface::return_type::OK; } @@ -182,20 +189,6 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta } } - // Not sure if I need this one, check it out - const std::string tf_prefix = freedrive_params_.tf_prefix; - { - const std::string interface_name = tf_prefix + "freedrive_mode_controller/freedrive_mode_transfer_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()) { - transfer_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; - } - } - return ControllerInterface::on_activate(state); } From ae0ae0ed2e31dfc0b47f511de371dcce04c407af Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sun, 13 Oct 2024 17:22:05 +0000 Subject: [PATCH 06/26] Added callback functions and clean up --- .../src/freedrive_mode_controller.cpp | 110 ++++++++++++++---- 1 file changed, 86 insertions(+), 24 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 7b606795c..bf851a507 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -52,8 +52,14 @@ controller_interface::CallbackReturn FreedriveModeController::on_init() return CallbackReturn::ERROR; } + auto joint_names = passthrough_params_.joints; + joint_names_.writeFromNonRT(joint_names); + number_of_joints_ = joint_names.size(); + state_interface_types_ = passthrough_params_.state_interfaces; + return controller_interface::CallbackReturn::SUCCESS; } + controller_interface::InterfaceConfiguration FreedriveModeController::command_interface_configuration() const { controller_interface::InterfaceConfiguration config; @@ -81,35 +87,11 @@ controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeContro controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - // I'm not sure I need these interfaces std::copy(joint_state_interface_names_.cbegin(), joint_state_interface_names_.cend(), std::back_inserter(config.names)); - // This doesn't exist for me, so I will comment it for now - //config.names.push_back(freedrive_params_.speed_scaling_interface_name); - return config; } -controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& /*period*/) -{ - const auto active_goal = *rt_active_goal_.readFromRT(); - - if (active_goal && trajectory_active_) { - // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the teach - // pendant. - if (abort_command_interface_->get().get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); - std::shared_ptr result = - std::make_shared(); - active_goal->setAborted(result); - end_goal(); - return controller_interface::return_type::OK; - } - } - return controller_interface::return_type::OK; -} - controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) { @@ -208,6 +190,86 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S return CallbackReturn::SUCCESS; } +controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + const auto active_goal = *rt_active_goal_.readFromRT(); + + if (active_goal && trajectory_active_) { + // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the teach + // pendant. + if (abort_command_interface_->get().get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + end_goal(); + return controller_interface::return_type::OK; + } + } + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); + // Precondition: Running controller + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't enable freedrive mode. Freedrive mode controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (freedrive_active_) { + RCLCPP_ERROR(get_node()->get_logger(), "Freedrive mode is already enabled: ignoring new request."); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_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(), "Disabling freedrive mode requested."); + + // Mark the current goal as canceled + auto result = std::make_shared(); + active_goal->setCanceled(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + freedrive_active_ = false; + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PassthroughTrajectoryController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); + + // Action handling will be done from the timer callback to avoid those things in the realtime + // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new + // one. + // + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + freedrive_active_ = true; + return; +} + +void FreedriveModeController::end_goal() +{ + freedrive_active_ = false; +} + void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { // Process the freedrive_mode command. From fb684f8a6edf5a8de89925643b5f87cf8728d52b Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sun, 13 Oct 2024 20:11:03 +0000 Subject: [PATCH 07/26] First version of activation/deactivation of freedrive --- .../src/freedrive_mode_controller.cpp | 76 +++++++++++++++++-- 1 file changed, 70 insertions(+), 6 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index bf851a507..d11fc3cb3 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -76,8 +76,8 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in config.names.push_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_abort"); // Get the command interfaces needed for freedrive mode from the hardware interface - // config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_async_success"); - // config.names.emplace_back(tf_prefix + "freedrive_mode/freedrive_mode_cmd"); + config.names.emplace_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_async_success"); + config.names.emplace_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_disable_cmd"); return config; } @@ -171,6 +171,32 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta } } + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" + "freedrive_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_controller/" + "freedrive_mode_disable_cmd"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + disable_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; + } + } + return ControllerInterface::on_activate(state); } @@ -195,11 +221,11 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat { const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal && trajectory_active_) { + if (active_goal && 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 (abort_command_interface_->get().get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action."); std::shared_ptr result = std::make_shared(); active_goal->setAborted(result); @@ -214,7 +240,7 @@ rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr goal) { - RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); + RCLCPP_INFO(get_node()->get_logger(), "Received new request for freedrive mode activation."); // Precondition: Running controller if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(get_node()->get_logger(), "Can't enable freedrive mode. Freedrive mode controller is not running."); @@ -237,6 +263,22 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca if (active_goal && active_goal->gh_ == goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Disabling freedrive mode requested."); + // Setting interfaces to deactivate freedrive mode + async_success_command_interface_->get().set_value(ASYNC_WAITING); + disable_command_interface_->get().set_value(1.0); + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); + while (async_success_command_interface_->get().get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the freedrive mode + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + bool success = static_cast(async_success_command_interface_->get().get_value()); + if (success) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not disable freedrive mode."); + } + // Mark the current goal as canceled auto result = std::make_shared(); active_goal->setCanceled(result); @@ -251,6 +293,28 @@ void PassthroughTrajectoryController::goal_accepted_callback( { RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); + // reset success flag + async_success_command_interface_->get().set_value(ASYNC_WAITING); + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); + // const auto maximum_retries = freedrive_params_.check_io_successful_retries; + // int retries = 0; + while (async_success_command_interface_->get().get_value() == ASYNC_WAITING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + // retries++; + + // if (retries > maximum_retries) { + // resp->success = false; + // } + } + + resp->success = static_cast(async_success_command_interface_->get().get_value()); + + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been set successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); + } // Action handling will be done from the timer callback to avoid those things in the realtime // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new // one. @@ -317,7 +381,7 @@ bool FreedriveModeController::enableFreedriveMode() bool FreedriveModeController::disableFreedriveMode() { command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); - command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_CMD].set_value(1); + command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_DISABLE_CMD].set_value(1); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); while (command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { From 9b8851756c602f8e69c684c33e30d6d197534a8f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 08:41:40 +0000 Subject: [PATCH 08/26] Updated to custom action EnableFreedriveMode --- .../freedrive_mode_controller.hpp | 17 ++++++++++---- .../src/freedrive_mode_controller.cpp | 22 +++++++++---------- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 6f11c57be..3b6b502a7 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -88,17 +88,27 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::vector> joint_velocity_state_interface_; // Everything related to the RT action server - using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; - using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using FreedriveModeAction = ur_msgs::action::EnableFreedriveMode; + 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. rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal realtime_tools::RealtimeBuffer> joint_trajectory_mapping_; - rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + rclcpp_action::Server::SharedPtr freedrive_mode_action_server_; + rclcpp_action::GoalResponse + goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + + void goal_accepted_callback( + std::shared_ptr> goal_handle); + // Not sure this is needed anymore, for tf_prefix there are other ways to handle // std::unique_ptr tf_buffer_; // std::unique_ptr tf_listener_; @@ -113,7 +123,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::atomic freedrive_mode_enable_; std::atomic freedrive_active_; - std::shared_ptr freedrive_mode_sub_; static constexpr double ASYNC_WAITING = 2.0; /** diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index d11fc3cb3..61c392550 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -128,7 +128,7 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St void FreedriveModeController::start_action_server(void) { - freedrive_mode_action_server_ = rclcpp_action::create_server( + freedrive_mode_action_server_ = rclcpp_action::create_server( get_node(), std::string(get_node()->get_name()) + "/freedrive_mode", std::bind(&FreedriveModeController::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), @@ -206,8 +206,8 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S abort_command_interface_->get().set_value(1.0); if (freedrive_active_) { const auto active_goal = *rt_active_goal_.readFromRT(); - std::shared_ptr result = - std::make_shared(); + std::shared_ptr result = + std::make_shared(); result->set__error_string("Deactivating freedrive mode, since the controller is being deactivated."); active_goal->setAborted(result); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); @@ -226,8 +226,8 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat // pendant. if (abort_command_interface_->get().get_value() == 1.0) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action."); - std::shared_ptr result = - std::make_shared(); + std::shared_ptr result = + std::make_shared(); active_goal->setAborted(result); end_goal(); return controller_interface::return_type::OK; @@ -238,7 +238,7 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( const rclcpp_action::GoalUUID& /*uuid*/, - std::shared_ptr goal) + std::shared_ptr goal) { RCLCPP_INFO(get_node()->get_logger(), "Received new request for freedrive mode activation."); // Precondition: Running controller @@ -255,8 +255,8 @@ rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( - const std::shared_ptr> goal_handle) +rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_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(); @@ -280,7 +280,7 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca } // Mark the current goal as canceled - auto result = std::make_shared(); + auto result = std::make_shared(); active_goal->setCanceled(result); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); freedrive_active_ = false; @@ -288,8 +288,8 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca return rclcpp_action::CancelResponse::ACCEPT; } -void PassthroughTrajectoryController::goal_accepted_callback( - std::shared_ptr> goal_handle) +void FreedriveModeController::goal_accepted_callback( + std::shared_ptr> goal_handle) { RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); From dafb640c66266ff91f057584d1a348484f09fc22 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 09:32:07 +0000 Subject: [PATCH 09/26] Removed previous version code --- .../freedrive_mode_controller.hpp | 5 -- .../src/freedrive_mode_controller.cpp | 64 ------------------- 2 files changed, 69 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 3b6b502a7..757da4469 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -75,10 +75,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface CallbackReturn on_init() override; private: - //void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); - //bool enableFreedriveMode(); - //bool disableFreedriveMode(); - // State interfaces realtime_tools::RealtimeBuffer> joint_names_; std::vector state_interface_types_; @@ -121,7 +117,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface void end_goal(); - std::atomic freedrive_mode_enable_; std::atomic freedrive_active_; static constexpr double ASYNC_WAITING = 2.0; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 61c392550..0567336a3 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -333,70 +333,6 @@ void FreedriveModeController::end_goal() { freedrive_active_ = false; } - -void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) -{ - // Process the freedrive_mode command. - if(msg->data) - { - freedrive_mode_enable_ = true; - RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); - } else{ - freedrive_mode_enable_ = false; - RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); - } -} - -bool FreedriveModeController::enableFreedriveMode() -{ - // reset success flag - command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); - - // Shouldn't I have a command set to 1 start it? Like it happens for the disable - - RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); - const auto maximum_retries = freedrive_params_.check_io_successful_retries; - int retries = 0; - while (command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - retries++; - - if (retries > maximum_retries) { - resp->success = false; - } - } - - resp->success = static_cast(command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value()); - - if (resp->success) { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been set successfully."); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); - return false; - } - - return true; -} - -bool FreedriveModeController::disableFreedriveMode() -{ - command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); - command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_DISABLE_CMD].set_value(1); - - RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); - while (command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { - // Asynchronous wait until the hardware interface has set the freedrive mode - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - bool success = static_cast(command_interfaces_[CommandInterfaces::FREEDRIVE_MODE_ASYNC_SUCCESS].get_value()); - if (success) { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Could not disable freedrive mode."); - return false; - } - return true; -} } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp" From a3902a618daf3440fc1890c9f6b87c86da1bd2e9 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 12:27:14 +0000 Subject: [PATCH 10/26] Adding retries for the sleep --- .../src/freedrive_mode_controller.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 0567336a3..15fd889bd 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -268,9 +268,16 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( disable_command_interface_->get().set_value(1.0); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); + const auto maximum_retries = freedrive_params_.check_io_successful_retries; + int retries = 0; while (async_success_command_interface_->get().get_value() == ASYNC_WAITING) { // Asynchronous wait until the hardware interface has set the freedrive mode std::this_thread::sleep_for(std::chrono::milliseconds(50)); + retries++; + + if (retries > maximum_retries) { + resp->success = false; + } } bool success = static_cast(async_success_command_interface_->get().get_value()); if (success) { @@ -297,15 +304,15 @@ void FreedriveModeController::goal_accepted_callback( async_success_command_interface_->get().set_value(ASYNC_WAITING); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); - // const auto maximum_retries = freedrive_params_.check_io_successful_retries; - // int retries = 0; + const auto maximum_retries = freedrive_params_.check_io_successful_retries; + int retries = 0; while (async_success_command_interface_->get().get_value() == ASYNC_WAITING) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); - // retries++; + retries++; - // if (retries > maximum_retries) { - // resp->success = false; - // } + if (retries > maximum_retries) { + resp->success = false; + } } resp->success = static_cast(async_success_command_interface_->get().get_value()); From 469ee940d842dc0e3bdec28559e7869fa585e9ff Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 14:06:06 +0000 Subject: [PATCH 11/26] Added missing includes --- .../freedrive_mode_controller.hpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 757da4469..512f4fcbd 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -37,12 +37,26 @@ #define UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ #pragma once + +#include +#include + #include #include #include +#include +#include #include #include +#include +#include +#include +#include +#include + +#include +#include "freedrive_mode_controller_parameters.hpp" namespace ur_controllers { @@ -82,6 +96,9 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::vector joint_state_interface_names_; std::vector> joint_position_state_interface_; std::vector> joint_velocity_state_interface_; + std::optional> abort_command_interface_; + std::optional> async_success_command_interface_; + std::optional> disable_command_interface_; // Everything related to the RT action server using FreedriveModeAction = ur_msgs::action::EnableFreedriveMode; @@ -109,8 +126,8 @@ class FreedriveModeController : public controller_interface::ControllerInterface // std::unique_ptr tf_buffer_; // std::unique_ptr tf_listener_; - std::shared_ptr param_listener_; - freedrive_mode_controller::Params params_; + std::shared_ptr freedrive_param_listener_; + freedrive_mode_controller::Params freedrive_params_; /* Start an action server with an action called: /freedrive_mode_controller/start_freedrive_mode. */ void start_action_server(void); From ad80107e747cd6caed4229a37b99df62e635b23a Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 14:06:45 +0000 Subject: [PATCH 12/26] Fixed wrong names and missing declarations --- ur_controllers/src/freedrive_mode_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 15fd889bd..8b6716584 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -52,10 +52,10 @@ controller_interface::CallbackReturn FreedriveModeController::on_init() return CallbackReturn::ERROR; } - auto joint_names = passthrough_params_.joints; + auto joint_names = freedrive_params_.joints; joint_names_.writeFromNonRT(joint_names); number_of_joints_ = joint_names.size(); - state_interface_types_ = passthrough_params_.state_interfaces; + state_interface_types_ = freedrive_params_.state_interfaces; return controller_interface::CallbackReturn::SUCCESS; } @@ -110,7 +110,7 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St freedrive_param_listener_->refresh_dynamic_parameters(); // get parameters from the listener in case they were updated - freedrive_params_ = /freedrive_param_listener_->get_params(); + freedrive_params_ = freedrive_param_listener_->get_params(); // Joint interfaces handling joint_state_interface_names_.clear(); @@ -173,7 +173,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" - "freedrive_async_success"; + "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()) { From 910990f53871eff1d863cbfbc09fb7f778c5489f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 14:07:39 +0000 Subject: [PATCH 13/26] Added parameters and freedrive_mode_controller to list --- ur_controllers/CMakeLists.txt | 8 ++++++++ ur_controllers/controller_plugins.xml | 5 +++++ .../src/freedrive_mode_controller_parameters.yaml | 12 ++++++++++++ ur_robot_driver/config/ur_controllers.yaml | 4 ++++ ur_robot_driver/urdf/ur.ros2_control.xacro | 6 ++++++ 5 files changed, 35 insertions(+) create mode 100644 ur_controllers/src/freedrive_mode_controller_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 7c1825273..803d86dd0 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(tf2_ros REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(action_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS angles @@ -36,6 +37,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ur_dashboard_msgs ur_msgs generate_parameter_library + action_msgs ) include_directories(include) @@ -56,6 +58,11 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + freedrive_mode_controller_parameters + src/freedrive_mode_controller_parameters.yaml +) + add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp @@ -69,6 +76,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + freedrive_mode_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 f0058ab55..52649df35 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + This controller handles the activation of the freedrive mode. + + 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..cd58c0cf5 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -0,0 +1,12 @@ +--- +freedrive_mode_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + check_io_successful_retries: { + type: int, + default_value: 10, + description: "Amount of retries for checking if setting force_mode was successful" + } \ No newline at end of file diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a512dc1ca..1ab5ffb95 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -124,3 +124,7 @@ forward_position_controller: - $(var tf_prefix)wrist_1_joint - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint + +freedrive_mode_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" \ No newline at end of file diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 580107df6..8e7879cbc 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -224,6 +224,12 @@ + + + + + + From 475b7610b58562b6bddeee0ee800aab5306b57dc Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 14 Oct 2024 14:08:30 +0000 Subject: [PATCH 14/26] Added interfaces to hardware_interface --- .../include/ur_robot_driver/hardware_interface.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) 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 67e27a222..0251b35f4 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -192,6 +192,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double system_interface_initialized_; bool async_thread_shutdown_; + // Freedrive mode controller interface values + double freedrive_mode_async_success_; + double freedrive_mode_disable_cmd_; + double freedrive_mode_abort_; + // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -228,6 +233,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::atomic_bool rtde_comm_has_been_started_ = false; urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); + + // Check if name is correct here + const std::string FREEDRIVE_MODE_CONTROLLER = "freedrive_mode"; }; } // namespace ur_robot_driver From 9aca570b0caa7257c6b7519622146afc96e48875 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 15 Oct 2024 08:11:37 +0000 Subject: [PATCH 15/26] Removed useless dependencies from old version --- ur_controllers/CMakeLists.txt | 2 -- .../include/ur_controllers/freedrive_mode_controller.hpp | 6 ------ 2 files changed, 8 deletions(-) diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 803d86dd0..85c1cb25b 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(tf2_ros REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) @@ -33,7 +32,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools std_msgs std_srvs - tf2_ros ur_dashboard_msgs ur_msgs generate_parameter_library diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 512f4fcbd..8671ced23 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -42,8 +42,6 @@ #include #include -#include -#include #include #include @@ -122,10 +120,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface void goal_accepted_callback( std::shared_ptr> goal_handle); - // Not sure this is needed anymore, for tf_prefix there are other ways to handle - // std::unique_ptr tf_buffer_; - // std::unique_ptr tf_listener_; - std::shared_ptr freedrive_param_listener_; freedrive_mode_controller::Params freedrive_params_; From 5883fcbc441cb799a5f4e138920c746f1e4dbc5b Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 15 Oct 2024 08:14:14 +0000 Subject: [PATCH 16/26] Removed joint-related command and state interfaces --- .../freedrive_mode_controller.hpp | 8 +--- .../src/freedrive_mode_controller.cpp | 46 +------------------ 2 files changed, 3 insertions(+), 51 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 8671ced23..256fac32b 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -87,13 +87,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface CallbackReturn on_init() override; private: - // State interfaces - realtime_tools::RealtimeBuffer> joint_names_; - std::vector state_interface_types_; - - std::vector joint_state_interface_names_; - std::vector> joint_position_state_interface_; - std::vector> joint_velocity_state_interface_; + // Command interfaces std::optional> abort_command_interface_; std::optional> async_success_command_interface_; std::optional> disable_command_interface_; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 8b6716584..25a278d4f 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -52,11 +52,6 @@ controller_interface::CallbackReturn FreedriveModeController::on_init() return CallbackReturn::ERROR; } - auto joint_names = freedrive_params_.joints; - joint_names_.writeFromNonRT(joint_names); - number_of_joints_ = joint_names.size(); - state_interface_types_ = freedrive_params_.state_interfaces; - return controller_interface::CallbackReturn::SUCCESS; } @@ -67,12 +62,6 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in const std::string tf_prefix = freedrive_params_.tf_prefix; - auto joint_names = freedrive_params_.joints; - for (auto& joint_name : joint_names) { - config.names.emplace_back(joint_name + "/position"); //hardware_interface::HW_IF_POSITION - config.names.emplace_back(joint_name + "/velocity"); //hardware_interface::HW_IF_VELOCITY - } - config.names.push_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_abort"); // Get the command interfaces needed for freedrive mode from the hardware interface @@ -85,9 +74,7 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - std::copy(joint_state_interface_names_.cbegin(), joint_state_interface_names_.cend(), std::back_inserter(config.names)); + config.type = controller_interface::interface_configuration_type::NONE; return config; } @@ -112,17 +99,6 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St // get parameters from the listener in case they were updated freedrive_params_ = freedrive_param_listener_->get_params(); - // Joint interfaces handling - joint_state_interface_names_.clear(); - - joint_state_interface_names_.reserve(number_of_joints_ * state_interface_types_.size()); - - auto joint_names_internal = joint_names_.readFromRT(); - for (const auto& joint_name : *joint_names_internal) { - for (const auto& interface_type : state_interface_types_) { - joint_state_interface_names_.emplace_back(joint_name + "/" + interface_type); - } - } return ControllerInterface::on_configure(previous_state); } @@ -141,23 +117,6 @@ controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& state) { - // clear out vectors in case of restart - joint_position_state_interface_.clear(); - joint_velocity_state_interface_.clear(); - - for (auto& interface_name : joint_state_interface_names_) { - auto interface_it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), - [&](auto& interface) { return (interface.get_name() == interface_name); }); - if (interface_it != state_interfaces_.end()) { - if (interface_it->get_interface_name() == "position") { - joint_position_state_interface_.emplace_back(*interface_it); - - } else if (interface_it->get_interface_name() == "velocity") { - joint_velocity_state_interface_.emplace_back(*interface_it); - } - } - } - { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" "freedrive_mode_abort"; @@ -344,5 +303,4 @@ void FreedriveModeController::end_goal() #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) -find_package(cartesian_controllers REQUIRED) +PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) \ No newline at end of file From 9d62717bfa7760fa3d444516c96d0c76ba8aa69e Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 15 Oct 2024 08:45:22 +0000 Subject: [PATCH 17/26] Added WaitForAsyncCommand --- .../src/freedrive_mode_controller.cpp | 45 ++++++++++--------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 25a278d4f..98d021dd9 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -227,17 +227,11 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( disable_command_interface_->get().set_value(1.0); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); - const auto maximum_retries = freedrive_params_.check_io_successful_retries; - int retries = 0; - while (async_success_command_interface_->get().get_value() == ASYNC_WAITING) { - // Asynchronous wait until the hardware interface has set the freedrive mode - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - retries++; - - if (retries > maximum_retries) { - resp->success = false; - } + if (!waitForAsyncCommand( + [&]() { return async_success_command_interface_->get().get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify that freedrive mode has been deactivated."); } + bool success = static_cast(async_success_command_interface_->get().get_value()); if (success) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); @@ -263,20 +257,13 @@ void FreedriveModeController::goal_accepted_callback( async_success_command_interface_->get().set_value(ASYNC_WAITING); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); - const auto maximum_retries = freedrive_params_.check_io_successful_retries; - int retries = 0; - while (async_success_command_interface_->get().get_value() == ASYNC_WAITING) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - retries++; - - if (retries > maximum_retries) { - resp->success = false; - } + if (!waitForAsyncCommand( + [&]() { return async_success_command_interface_->get().get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify that freedrive mode has been activated."); } - resp->success = static_cast(async_success_command_interface_->get().get_value()); - - if (resp->success) { + bool success = static_cast(async_success_command_interface_->get().get_value()); + if (success) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been set successfully."); } else { RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); @@ -299,6 +286,20 @@ void FreedriveModeController::end_goal() { freedrive_active_ = 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" From 816f5a30157ead1d5c25a10ab98b2e879a0691cd Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 15 Oct 2024 09:00:27 +0000 Subject: [PATCH 18/26] Missing dependencies and removed result reference --- .../include/ur_controllers/freedrive_mode_controller.hpp | 3 ++- ur_controllers/src/freedrive_mode_controller.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 256fac32b..07f7a8a22 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -68,6 +68,8 @@ enum StateInterfaces INITIALIZED_FLAG = 0u, }; +using namespace std::chrono_literals; // NOLINT + class FreedriveModeController : public controller_interface::ControllerInterface { public: @@ -119,7 +121,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface /* Start an action server with an action called: /freedrive_mode_controller/start_freedrive_mode. */ void start_action_server(void); - void end_goal(); std::atomic freedrive_active_; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 98d021dd9..95eec37d6 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -34,10 +34,12 @@ * \date 2024-09-16 */ //---------------------------------------------------------------------- - +#include +#include #include - +#include #include + namespace ur_controllers { controller_interface::CallbackReturn FreedriveModeController::on_init() @@ -167,7 +169,7 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S const auto active_goal = *rt_active_goal_.readFromRT(); std::shared_ptr result = std::make_shared(); - result->set__error_string("Deactivating freedrive mode, since the controller is being deactivated."); + //result->set__error_string("Deactivating freedrive mode, since the controller is being deactivated."); active_goal->setAborted(result); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); end_goal(); From 4931a59dabddb88bd4c12deb202f2a18035eb85a Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 16 Oct 2024 08:14:25 +0000 Subject: [PATCH 19/26] Added freedrive to launch files --- ur_controllers/package.xml | 1 + ur_robot_driver/config/ur_controllers.yaml | 2 ++ ur_robot_driver/launch/ur10.launch.py | 1 + ur_robot_driver/launch/ur10e.launch.py | 1 + ur_robot_driver/launch/ur16e.launch.py | 1 + ur_robot_driver/launch/ur20.launch.py | 1 + ur_robot_driver/launch/ur3.launch.py | 1 + ur_robot_driver/launch/ur30.launch.py | 1 + ur_robot_driver/launch/ur3e.launch.py | 1 + ur_robot_driver/launch/ur5.launch.py | 1 + ur_robot_driver/launch/ur5e.launch.py | 1 + ur_robot_driver/launch/ur_control.launch.py | 15 ++++++++++++++- 12 files changed, 26 insertions(+), 1 deletion(-) diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 98f304df8..7e7ac420e 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -33,6 +33,7 @@ std_srvs ur_dashboard_msgs ur_msgs + action_msgs ament_cmake diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 1ab5ffb95..cedda25ca 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,8 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + freedrive_mode_controller: + type: ur_controllers/FreedriveModeController speed_scaling_state_broadcaster: ros__parameters: diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 8bbc698db..c593f1c9f 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", ], ) ) diff --git a/ur_robot_driver/launch/ur10e.launch.py b/ur_robot_driver/launch/ur10e.launch.py index 024712440..1f0909a2b 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", ], ) ) diff --git a/ur_robot_driver/launch/ur16e.launch.py b/ur_robot_driver/launch/ur16e.launch.py index f10d56c3b..8e1515ad2 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", ], ) ) diff --git a/ur_robot_driver/launch/ur20.launch.py b/ur_robot_driver/launch/ur20.launch.py index a5f8afc4d..a4984f515 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", ], ) ) diff --git a/ur_robot_driver/launch/ur3.launch.py b/ur_robot_driver/launch/ur3.launch.py index d37e50dd9..e98934b84 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", ], ) ) diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py index fcbac536b..133d7e8fb 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", ], ) ) diff --git a/ur_robot_driver/launch/ur3e.launch.py b/ur_robot_driver/launch/ur3e.launch.py index 87e5a949d..22159e6ce 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", ], ) ) diff --git a/ur_robot_driver/launch/ur5.launch.py b/ur_robot_driver/launch/ur5.launch.py index 6a1439693..a0afd9a49 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", ], ) ) diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index df36d7e1d..3dee84522 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", ], ) ) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c884086fa..cbdd6957c 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -163,7 +163,13 @@ def controller_spawner(controllers, active=True): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controllers_inactive = ["forward_position_controller"] + controllers_inactive = [ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + ] controller_spawners = [controller_spawner(controllers_active)] + [ controller_spawner(controllers_inactive, active=False) @@ -328,6 +334,13 @@ def generate_launch_description(): DeclareLaunchArgument( "initial_joint_controller", default_value="scaled_joint_trajectory_controller", + choices=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_velocity_controller", + "forward_position_controller", + "freedrive_mode_controller", + ], description="Initially loaded robot controller.", ) ) From 7719944ecd72fe2a844d1bb38fe9988900b4a3a6 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 16 Oct 2024 08:15:08 +0000 Subject: [PATCH 20/26] First version of the hardware interface --- .../ur_robot_driver/hardware_interface.hpp | 6 +- ur_robot_driver/src/hardware_interface.cpp | 62 ++++++++++++++++++- 2 files changed, 65 insertions(+), 3 deletions(-) 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 0251b35f4..7746e08f7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -77,7 +77,8 @@ enum StoppingInterface { NONE, STOP_POSITION, - STOP_VELOCITY + STOP_VELOCITY, + STOP_FREEDRIVE }; /*! @@ -193,6 +194,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool async_thread_shutdown_; // Freedrive mode controller interface values + bool freedrive_mode_controller_running_; double freedrive_mode_async_success_; double freedrive_mode_disable_cmd_; double freedrive_mode_abort_; @@ -235,7 +237,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); // Check if name is correct here - const std::string FREEDRIVE_MODE_CONTROLLER = "freedrive_mode"; + const std::string FREEDRIVE_MODE = "freedrive_mode_controller"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5826cf595..03a170512 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -85,6 +85,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; + freedrive_mode_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -93,6 +94,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; for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -298,6 +300,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_controller", "freedrive_mode_async_success", &freedrive_mode_async_success_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "freedrive_mode_controller", "freedrive_mode_disable_cmd", &freedrive_mode_disable_cmd_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "freedrive_mode_controller", "freedrive_mode_abort", &freedrive_mode_abort_)); + return command_interfaces; } @@ -631,6 +642,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_) { + ur_driver_->writeKeepalive(); + } else { ur_driver_->writeKeepalive(); } @@ -801,7 +815,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod start_modes_.clear(); stop_modes_.clear(); - + const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); // Starting interfaces // add start interface per joint in tmp var for later check for (const auto& key : start_interfaces) { @@ -812,6 +826,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } + if (key == tf_prefix + FREEDRIVE_MODE + "/freedrive_mode_async_success") { + start_modes_.push_back(FREEDRIVE_MODE); + } } } // set new mode to all interfaces at the same time @@ -819,6 +836,39 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod ret_val = hardware_interface::return_type::ERROR; } + if (position_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_POSITION; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == FREEDRIVE_MODE); + })) { + RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position " + "interface running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (velocity_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_POSITION || item == FREEDRIVE_MODE); + })) { + RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity " + "interface running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (freedrive_mode_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_FREEDRIVE; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + })) { + RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while the freedrive controller " + "is running."); + ret_val = hardware_interface::return_type::ERROR; + } + // all start interfaces must be the same - can't mix position and velocity control if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { ret_val = hardware_interface::return_type::ERROR; @@ -859,6 +909,10 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), + StoppingInterface::STOP_FREEDRIVE) != stop_modes_.end()) { + freedrive_mode_controller_running_ = false; + freedrive_mode_abort_ = 1.0; } if (start_modes_.size() != 0 && @@ -872,6 +926,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod position_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_.size() != 0 && + std::find(start_modes_.begin(), start_modes_.end(), FREEDRIVE_MODE) != start_modes_.end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + freedrive_mode_controller_running_ = true; + freedrive_mode_abort_ = 0.0; } start_modes_.clear(); From 409a9e8e64b2a834accd5537e9919ddd60d9f2be Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 16 Oct 2024 10:47:26 +0000 Subject: [PATCH 21/26] Updated command interfaces and fixed overlapping name --- .../freedrive_mode_controller.hpp | 1 - .../src/freedrive_mode_controller.cpp | 29 +++++-------------- .../ur_robot_driver/hardware_interface.hpp | 3 +- ur_robot_driver/src/hardware_interface.cpp | 13 ++++----- ur_robot_driver/urdf/ur.ros2_control.xacro | 7 ++--- 5 files changed, 16 insertions(+), 37 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 07f7a8a22..07b93ee0e 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -92,7 +92,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface // Command interfaces std::optional> abort_command_interface_; std::optional> async_success_command_interface_; - std::optional> disable_command_interface_; // Everything related to the RT action server using FreedriveModeAction = ur_msgs::action::EnableFreedriveMode; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 95eec37d6..cf0e080c2 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -64,11 +64,9 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in const std::string tf_prefix = freedrive_params_.tf_prefix; - config.names.push_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_abort"); - // Get the command interfaces needed for freedrive mode from the hardware interface - config.names.emplace_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_async_success"); - config.names.emplace_back(tf_prefix + "freedrive_mode_controller/freedrive_mode_disable_cmd"); + config.names.emplace_back(tf_prefix + "freedrive_mode/async_success"); + config.names.emplace_back(tf_prefix + "freedrive_mode/abort"); return config; } @@ -120,8 +118,8 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { { - const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" - "freedrive_mode_abort"; + 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()) { @@ -133,8 +131,8 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta } { - const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" - "freedrive_mode_async_success"; + 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()) { @@ -145,19 +143,6 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta } } - { - const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode_controller/" - "freedrive_mode_disable_cmd"; - auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), - [&](auto& interface) { return (interface.get_name() == interface_name); }); - if (it != command_interfaces_.end()) { - disable_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; - } - } - return ControllerInterface::on_activate(state); } @@ -226,7 +211,7 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( // Setting interfaces to deactivate freedrive mode async_success_command_interface_->get().set_value(ASYNC_WAITING); - disable_command_interface_->get().set_value(1.0); + abort_command_interface_->get().set_value(1.0); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); if (!waitForAsyncCommand( 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 7746e08f7..0e478f7fa 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -196,7 +196,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // Freedrive mode controller interface values bool freedrive_mode_controller_running_; double freedrive_mode_async_success_; - double freedrive_mode_disable_cmd_; double freedrive_mode_abort_; // payload stuff @@ -237,7 +236,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); // Check if name is correct here - const std::string FREEDRIVE_MODE = "freedrive_mode_controller"; + const std::string FREEDRIVE_MODE = "freedrive_mode"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 03a170512..4a25e5a31 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -301,13 +301,10 @@ std::vector URPositionHardwareInterface::e tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "freedrive_mode_controller", "freedrive_mode_async_success", &freedrive_mode_async_success_)); + tf_prefix + FREEDRIVE_MODE, "async_success", &freedrive_mode_async_success_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "freedrive_mode_controller", "freedrive_mode_disable_cmd", &freedrive_mode_disable_cmd_)); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "freedrive_mode_controller", "freedrive_mode_abort", &freedrive_mode_abort_)); + tf_prefix + FREEDRIVE_MODE, "abort", &freedrive_mode_abort_)); return command_interfaces; } @@ -826,7 +823,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } - if (key == tf_prefix + FREEDRIVE_MODE + "/freedrive_mode_async_success") { + if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { start_modes_.push_back(FREEDRIVE_MODE); } } @@ -842,7 +839,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == FREEDRIVE_MODE); })) { - RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position " + RCLCPP_ERROR(get_logger(), "Start of velocity interface or freedrive mode requested while there is the position " "interface running."); ret_val = hardware_interface::return_type::ERROR; } @@ -853,7 +850,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_POSITION || item == FREEDRIVE_MODE); })) { - RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity " + RCLCPP_ERROR(get_logger(), "Start of position interface or freedrive mode requested while there is the velocity " "interface running."); ret_val = hardware_interface::return_type::ERROR; } diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 8e7879cbc..477338874 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -224,10 +224,9 @@ - - - - + + + From 24f644b080257cb259e4d445764cefe5bfe0899f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 16 Oct 2024 17:35:36 +0000 Subject: [PATCH 22/26] Added enable interface --- .../freedrive_mode_controller.hpp | 5 ++++- .../src/freedrive_mode_controller.cpp | 21 ++++++++++++++++--- .../ur_robot_driver/hardware_interface.hpp | 2 ++ ur_robot_driver/urdf/ur.ros2_control.xacro | 1 + 4 files changed, 25 insertions(+), 4 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 07b93ee0e..49c1703c8 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -90,8 +90,9 @@ class FreedriveModeController : public controller_interface::ControllerInterface private: // Command interfaces - std::optional> abort_command_interface_; std::optional> async_success_command_interface_; + std::optional> enable_command_interface_; + std::optional> abort_command_interface_; // Everything related to the RT action server using FreedriveModeAction = ur_msgs::action::EnableFreedriveMode; @@ -123,6 +124,8 @@ class FreedriveModeController : public controller_interface::ControllerInterface void end_goal(); std::atomic freedrive_active_; + std::atomic change_requested_; + std::atomic async_state_; static constexpr double ASYNC_WAITING = 2.0; /** diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index cf0e080c2..8f835e645 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -66,6 +66,7 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in // 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; @@ -119,11 +120,12 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" - "abort"; + "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()) { abort_command_interface_ = *it; + 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; @@ -132,11 +134,24 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" - "async_success"; + "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()) { - async_success_command_interface_ = *it; + 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; } else { RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); return controller_interface::CallbackReturn::ERROR; 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 0e478f7fa..9903ec24f 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -194,8 +194,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool async_thread_shutdown_; // Freedrive mode controller interface values + bool freedrive_action_requested_; bool freedrive_mode_controller_running_; double freedrive_mode_async_success_; + double freedrive_mode_enable_; double freedrive_mode_abort_; // payload stuff diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 477338874..2ffac4c64 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -226,6 +226,7 @@ + From e952d5f575aac7d32eefd60d015431e22ac41b7f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 16 Oct 2024 17:36:19 +0000 Subject: [PATCH 23/26] Update logic --- .../src/freedrive_mode_controller.cpp | 97 +++++++++++++------ ur_robot_driver/src/hardware_interface.cpp | 25 ++++- 2 files changed, 88 insertions(+), 34 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 8f835e645..d69947a33 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -85,7 +85,6 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St { start_action_server(); - freedrive_active_ = false; const auto logger = get_node()->get_logger(); @@ -117,6 +116,9 @@ void FreedriveModeController::start_action_server(void) 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(); { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" @@ -124,7 +126,6 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta 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; async_success_command_interface_ = *it; } else { RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); @@ -165,14 +166,14 @@ controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) { abort_command_interface_->get().set_value(1.0); - if (freedrive_active_) { - const auto active_goal = *rt_active_goal_.readFromRT(); + + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) { std::shared_ptr result = std::make_shared(); //result->set__error_string("Deactivating freedrive mode, since the controller is being deactivated."); active_goal->setAborted(result); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - end_goal(); } return CallbackReturn::SUCCESS; } @@ -181,18 +182,39 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat const rclcpp::Duration& /*period*/) { const auto active_goal = *rt_active_goal_.readFromRT(); + async_state_ = async_success_command_interface_->get().get_value(); + + if(change_requested_) { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Change requested: either enabling or disabling freedrive."); + //if (active_goal && freedrive_active_) { + 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 (abort_command_interface_->get().get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + end_goal(); + return controller_interface::return_type::OK; + } else { + + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Change requested: ENABLING freedrive."); + // 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_STREAM(get_node()->get_logger(), "Change requested: DISABLING freedrive."); + abort_command_interface_->get().set_value(1.0); - if (active_goal && 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 (abort_command_interface_->get().get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action."); - std::shared_ptr result = - std::make_shared(); - active_goal->setAborted(result); - end_goal(); - return controller_interface::return_type::OK; + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; } + change_requested_ = false; } return controller_interface::return_type::OK; } @@ -219,22 +241,27 @@ rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( const std::shared_ptr> goal_handle) { + bool success; + // 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(), "Disabling freedrive mode requested."); // Setting interfaces to deactivate freedrive mode - async_success_command_interface_->get().set_value(ASYNC_WAITING); - abort_command_interface_->get().set_value(1.0); + //async_success_command_interface_->get().set_value(ASYNC_WAITING); + //abort_command_interface_->get().set_value(1.0); + + freedrive_active_ = false; + change_requested_ = true; - RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be disabled."); - if (!waitForAsyncCommand( - [&]() { return async_success_command_interface_->get().get_value(); })) { - RCLCPP_WARN(get_node()->get_logger(), "Could not verify that freedrive mode has been deactivated."); + RCLCPP_INFO(get_node()->get_logger(), "Waiting for the freedrive mode to be disabled."); + while (async_state_ == ASYNC_WAITING || change_requested_) { + // Asynchronous wait until the hardware interface has set the force mode + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - bool success = static_cast(async_success_command_interface_->get().get_value()); + success = async_state_ == 1.0; if (success) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); } else { @@ -255,18 +282,27 @@ void FreedriveModeController::goal_accepted_callback( { RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); - // reset success flag - async_success_command_interface_->get().set_value(ASYNC_WAITING); + bool success; + freedrive_active_ = true; + change_requested_ = true; - RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for freedrive mode to be set."); - if (!waitForAsyncCommand( - [&]() { return async_success_command_interface_->get().get_value(); })) { - RCLCPP_WARN(get_node()->get_logger(), "Could not verify that freedrive mode has been activated."); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Waiting for freedrive mode to be set."); + const auto maximum_retries = freedrive_params_.check_io_successful_retries; + int retries = 0; + while (async_state_ == ASYNC_WAITING || change_requested_) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + retries++; + + if (retries > maximum_retries) { + success = false; + } } - bool success = static_cast(async_success_command_interface_->get().get_value()); + // Check if the change was successful + success = async_state_ == 1.0; + if (success) { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been set successfully."); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Freedrive mode has been set successfully."); } else { RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); } @@ -280,7 +316,6 @@ void FreedriveModeController::goal_accepted_callback( goal_handle_timer_.reset(); goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); - freedrive_active_ = true; return; } diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 4a25e5a31..6b634350b 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -303,6 +303,9 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + FREEDRIVE_MODE, "async_success", &freedrive_mode_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + FREEDRIVE_MODE, "enable", &freedrive_mode_enable_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + FREEDRIVE_MODE, "abort", &freedrive_mode_abort_)); @@ -611,6 +614,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; + freedrive_mode_abort_ = NO_NEW_CMD_; + freedrive_mode_enable_ = NO_NEW_CMD_; initialized_ = true; } @@ -639,8 +644,8 @@ 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_) { - ur_driver_->writeKeepalive(); + } else if (freedrive_mode_controller_running_ && freedrive_action_requested_) { + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); } else { ur_driver_->writeKeepalive(); @@ -738,6 +743,19 @@ 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_STREAM(get_logger(), "Starting freedrive mode."); + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_enable_ = NO_NEW_CMD_; + freedrive_action_requested_ = true; + } + + if (!std::isnan(freedrive_mode_abort_) && freedrive_action_requested_ && ur_driver_ != nullptr) { + RCLCPP_INFO_STREAM(get_logger(), "Stopping freedrive mode."); + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_mode_abort_ = NO_NEW_CMD_; + } } void URPositionHardwareInterface::updateNonDoubleValues() @@ -909,6 +927,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_FREEDRIVE) != stop_modes_.end()) { freedrive_mode_controller_running_ = false; + freedrive_action_requested_ = false; freedrive_mode_abort_ = 1.0; } @@ -928,7 +947,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod velocity_controller_running_ = false; position_controller_running_ = false; freedrive_mode_controller_running_ = true; - freedrive_mode_abort_ = 0.0; + freedrive_action_requested_ = false; } start_modes_.clear(); From e819adaa4da943981350165bce08552beb21896a Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 16 Oct 2024 19:12:12 +0000 Subject: [PATCH 24/26] Fixed not updating async value --- ur_robot_driver/src/hardware_interface.cpp | 106 ++++++++++----------- 1 file changed, 53 insertions(+), 53 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 6b634350b..3a870258d 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -746,85 +746,85 @@ void URPositionHardwareInterface::checkAsyncIO() if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { RCLCPP_INFO_STREAM(get_logger(), "Starting freedrive mode."); - ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); freedrive_mode_enable_ = NO_NEW_CMD_; freedrive_action_requested_ = true; } if (!std::isnan(freedrive_mode_abort_) && freedrive_action_requested_ && ur_driver_ != nullptr) { RCLCPP_INFO_STREAM(get_logger(), "Stopping freedrive mode."); - ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); - freedrive_mode_abort_ = NO_NEW_CMD_; - } + freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_mode_abort_ = NO_NEW_CMD_; +} } void URPositionHardwareInterface::updateNonDoubleValues() { - for (size_t i = 0; i < 18; ++i) { - actual_dig_out_bits_copy_[i] = static_cast(actual_dig_out_bits_[i]); - actual_dig_in_bits_copy_[i] = static_cast(actual_dig_in_bits_[i]); - } +for (size_t i = 0; i < 18; ++i) { + actual_dig_out_bits_copy_[i] = static_cast(actual_dig_out_bits_[i]); + actual_dig_in_bits_copy_[i] = static_cast(actual_dig_in_bits_[i]); +} - for (size_t i = 0; i < 11; ++i) { - safety_status_bits_copy_[i] = static_cast(safety_status_bits_[i]); - } +for (size_t i = 0; i < 11; ++i) { + safety_status_bits_copy_[i] = static_cast(safety_status_bits_[i]); +} - for (size_t i = 0; i < 4; ++i) { - analog_io_types_copy_[i] = static_cast(analog_io_types_[i]); - robot_status_bits_copy_[i] = static_cast(robot_status_bits_[i]); - } +for (size_t i = 0; i < 4; ++i) { + analog_io_types_copy_[i] = static_cast(analog_io_types_[i]); + robot_status_bits_copy_[i] = static_cast(robot_status_bits_[i]); +} - for (size_t i = 0; i < 2; ++i) { - tool_analog_input_types_copy_[i] = static_cast(tool_analog_input_types_[i]); - } +for (size_t i = 0; i < 2; ++i) { + tool_analog_input_types_copy_[i] = static_cast(tool_analog_input_types_[i]); +} - tool_output_voltage_copy_ = static_cast(tool_output_voltage_); - robot_mode_copy_ = static_cast(robot_mode_); - safety_mode_copy_ = static_cast(safety_mode_); - tool_mode_copy_ = static_cast(tool_mode_); - system_interface_initialized_ = initialized_ ? 1.0 : 0.0; - robot_program_running_copy_ = robot_program_running_ ? 1.0 : 0.0; +tool_output_voltage_copy_ = static_cast(tool_output_voltage_); +robot_mode_copy_ = static_cast(robot_mode_); +safety_mode_copy_ = static_cast(safety_mode_); +tool_mode_copy_ = static_cast(tool_mode_); +system_interface_initialized_ = initialized_ ? 1.0 : 0.0; +robot_program_running_copy_ = robot_program_running_ ? 1.0 : 0.0; } void URPositionHardwareInterface::transformForceTorque() { - // imported from ROS1 driver - hardware_interface.cpp#L867-L876 - tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1], - urcl_ft_sensor_measurements_[2]); - tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], - urcl_ft_sensor_measurements_[5]); - - tf2::Quaternion rotation_quat; - tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat); - tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_); - tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_); - - urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), - tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; +// imported from ROS1 driver - hardware_interface.cpp#L867-L876 +tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1], + urcl_ft_sensor_measurements_[2]); +tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], + urcl_ft_sensor_measurements_[5]); + +tf2::Quaternion rotation_quat; +tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat); +tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_); +tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_); + +urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), + tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; } void URPositionHardwareInterface::extractToolPose() { - // imported from ROS1 driver hardware_interface.cpp#L911-L928 - double tcp_angle = - std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2)); - - tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]); - tf2::Quaternion rotation; - if (tcp_angle > 1e-16) { - rotation.setRotation(rotation_vec.normalized(), tcp_angle); - } else { - rotation.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid - } - tcp_transform_.transform.translation.x = urcl_tcp_pose_[0]; - tcp_transform_.transform.translation.y = urcl_tcp_pose_[1]; - tcp_transform_.transform.translation.z = urcl_tcp_pose_[2]; +// imported from ROS1 driver hardware_interface.cpp#L911-L928 +double tcp_angle = + std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2)); + +tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]); +tf2::Quaternion rotation; +if (tcp_angle > 1e-16) { + rotation.setRotation(rotation_vec.normalized(), tcp_angle); +} else { + rotation.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid +} +tcp_transform_.transform.translation.x = urcl_tcp_pose_[0]; +tcp_transform_.transform.translation.y = urcl_tcp_pose_[1]; +tcp_transform_.transform.translation.z = urcl_tcp_pose_[2]; - tcp_transform_.transform.rotation = tf2::toMsg(rotation); +tcp_transform_.transform.rotation = tf2::toMsg(rotation); } hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch( - const std::vector& start_interfaces, const std::vector& stop_interfaces) + const std::vector& start_interfaces, const std::vector& stop_interfaces) { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; From 9a3bad21ba9d8c229033eda0b5fff8999b8d694b Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Thu, 17 Oct 2024 08:50:47 +0000 Subject: [PATCH 25/26] Code cleanup --- .../freedrive_mode_controller.hpp | 1 - .../src/freedrive_mode_controller.cpp | 28 +++++-------------- ur_robot_driver/src/hardware_interface.cpp | 6 ++-- 3 files changed, 10 insertions(+), 25 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 49c1703c8..ab5472372 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -121,7 +121,6 @@ class FreedriveModeController : public controller_interface::ControllerInterface /* Start an action server with an action called: /freedrive_mode_controller/start_freedrive_mode. */ void start_action_server(void); - void end_goal(); std::atomic freedrive_active_; std::atomic change_requested_; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index d69947a33..343fc109a 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -44,7 +44,6 @@ namespace ur_controllers { controller_interface::CallbackReturn FreedriveModeController::on_init() { - // Even if the only param I use is tf_prefix, I still need it try { // Create the parameter listener and get the parameters freedrive_param_listener_ = std::make_shared(get_node()); @@ -93,10 +92,10 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St return controller_interface::CallbackReturn::ERROR; } - // update the dynamic map parameters + // Update the dynamic map parameters freedrive_param_listener_->refresh_dynamic_parameters(); - // get parameters from the listener in case they were updated + // Get parameters from the listener in case they were updated freedrive_params_ = freedrive_param_listener_->get_params(); return ControllerInterface::on_configure(previous_state); @@ -171,7 +170,6 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S if (active_goal) { std::shared_ptr result = std::make_shared(); - //result->set__error_string("Deactivating freedrive mode, since the controller is being deactivated."); active_goal->setAborted(result); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } @@ -185,8 +183,6 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat async_state_ = async_success_command_interface_->get().get_value(); if(change_requested_) { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Change requested: either enabling or disabling freedrive."); - //if (active_goal && freedrive_active_) { 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. @@ -195,11 +191,10 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat std::shared_ptr result = std::make_shared(); active_goal->setAborted(result); - end_goal(); + freedrive_active_ = false; return controller_interface::return_type::OK; } else { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Change requested: ENABLING freedrive."); // Set command interface to enable enable_command_interface_->get().set_value(1.0); @@ -208,7 +203,6 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat } } else { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Change requested: DISABLING freedrive."); abort_command_interface_->get().set_value(1.0); async_success_command_interface_->get().set_value(ASYNC_WAITING); @@ -224,6 +218,7 @@ rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( std::shared_ptr goal) { RCLCPP_INFO(get_node()->get_logger(), "Received new request for freedrive mode activation."); + // Precondition: Running controller if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(get_node()->get_logger(), "Can't enable freedrive mode. Freedrive mode controller is not running."); @@ -248,10 +243,6 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( if (active_goal && active_goal->gh_ == goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Disabling freedrive mode requested."); - // Setting interfaces to deactivate freedrive mode - //async_success_command_interface_->get().set_value(ASYNC_WAITING); - //abort_command_interface_->get().set_value(1.0); - freedrive_active_ = false; change_requested_ = true; @@ -272,7 +263,6 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( auto result = std::make_shared(); active_goal->setCanceled(result); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - freedrive_active_ = false; } return rclcpp_action::CancelResponse::ACCEPT; } @@ -283,10 +273,11 @@ void FreedriveModeController::goal_accepted_callback( RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); bool success; + freedrive_active_ = true; change_requested_ = true; - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Waiting for freedrive mode to be set."); + RCLCPP_INFO(get_node()->get_logger(), "Waiting for freedrive mode to be set."); const auto maximum_retries = freedrive_params_.check_io_successful_retries; int retries = 0; while (async_state_ == ASYNC_WAITING || change_requested_) { @@ -306,10 +297,10 @@ void FreedriveModeController::goal_accepted_callback( } else { RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); } + // Action handling will be done from the timer callback to avoid those things in the realtime // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new // one. - // RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); @@ -319,11 +310,6 @@ void FreedriveModeController::goal_accepted_callback( return; } -void FreedriveModeController::end_goal() -{ - freedrive_active_ = false; -} - bool FreedriveModeController::waitForAsyncCommand(std::function get_value) { const auto maximum_retries = freedrive_params_.check_io_successful_retries; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3a870258d..682969830 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -745,16 +745,16 @@ void URPositionHardwareInterface::checkAsyncIO() } if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { - RCLCPP_INFO_STREAM(get_logger(), "Starting freedrive mode."); + 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_action_requested_ = true; } if (!std::isnan(freedrive_mode_abort_) && freedrive_action_requested_ && ur_driver_ != nullptr) { - RCLCPP_INFO_STREAM(get_logger(), "Stopping freedrive mode."); + RCLCPP_INFO(get_logger(), "Stopping freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); - freedrive_mode_abort_ = NO_NEW_CMD_; + freedrive_mode_abort_ = NO_NEW_CMD_; } } From f302220818a7e090930df18c7d6a5aa6d4266733 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 18 Oct 2024 15:53:15 +0200 Subject: [PATCH 26/26] Make sure to reset the abort command interface when starting the controller --- ur_controllers/src/freedrive_mode_controller.cpp | 4 +++- ur_robot_driver/src/hardware_interface.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 343fc109a..0741ba339 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -152,6 +152,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta [&](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; @@ -186,7 +187,8 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat 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 (abort_command_interface_->get().get_value() == 1.0) { + 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 action."); std::shared_ptr result = std::make_shared(); diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 682969830..46309d9a4 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -751,9 +751,11 @@ void URPositionHardwareInterface::checkAsyncIO() freedrive_action_requested_ = true; } - if (!std::isnan(freedrive_mode_abort_) && freedrive_action_requested_ && ur_driver_ != nullptr) { + if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_action_requested_ && + ur_driver_ != nullptr) { RCLCPP_INFO(get_logger(), "Stopping freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_action_requested_ = false; freedrive_mode_abort_ = NO_NEW_CMD_; } }