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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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_; } } From b1b35b0fb819dc61ddc88d981db5c50016f918fc Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 18 Nov 2024 09:41:44 +0000 Subject: [PATCH 27/54] Removed legacy from action server version --- .../freedrive_mode_controller.hpp | 22 --------- .../src/freedrive_mode_controller.cpp | 46 +++---------------- ur_robot_driver/launch/ur_control.launch.py | 6 +-- 3 files changed, 8 insertions(+), 66 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index ab5472372..eeb235a34 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -53,7 +53,6 @@ #include #include -#include #include "freedrive_mode_controller_parameters.hpp" namespace ur_controllers @@ -94,34 +93,13 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::optional> enable_command_interface_; std::optional> abort_command_interface_; - // Everything related to the RT action server - 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); - 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); - std::atomic freedrive_active_; std::atomic change_requested_; std::atomic async_state_; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 0741ba339..1a8d83646 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -83,7 +83,6 @@ controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) { - start_action_server(); const auto logger = get_node()->get_logger(); @@ -101,17 +100,6 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St 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& state) { @@ -167,20 +155,12 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S { abort_command_interface_->get().set_value(1.0); - const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal) { - std::shared_ptr result = - std::make_shared(); - active_goal->setAborted(result); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } 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(); async_state_ = async_success_command_interface_->get().get_value(); if(change_requested_) { @@ -190,9 +170,6 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat 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(); - active_goal->setAborted(result); freedrive_active_ = false; return controller_interface::return_type::OK; } else { @@ -215,26 +192,13 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat 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 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."); - return rclcpp_action::GoalResponse::REJECT; + // Process the freedrive_mode command. } - - 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; } +// Timeout handling for the topic +/* rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( const std::shared_ptr> goal_handle) { @@ -268,7 +232,10 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( } return rclcpp_action::CancelResponse::ACCEPT; } +*/ +// Don't need this anymore, but logic must be reproduced in subscriber topic +/* void FreedriveModeController::goal_accepted_callback( std::shared_ptr> goal_handle) { @@ -311,6 +278,7 @@ void FreedriveModeController::goal_accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); return; } +*/ bool FreedriveModeController::waitForAsyncCommand(std::function get_value) { diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 3cbeef41f..b8f738564 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -175,12 +175,8 @@ def controller_spawner(controllers, active=True): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", - "freedrive_mode_controller", - ] - - controller_spawners = [controller_spawner(controllers_active)] + [ - controller_spawner(controllers_inactive, active=False) "passthrough_trajectory_controller", + "freedrive_mode_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) From cf50251218ff48bf98a6a235f74254a64f57dfef Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 18 Nov 2024 09:44:10 +0000 Subject: [PATCH 28/54] Fixed format from merge and removed old version --- ur_robot_driver/src/hardware_interface.cpp | 101 ++++++--------------- 1 file changed, 30 insertions(+), 71 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index fc70cea7f..a0038e129 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -348,6 +348,7 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + FREEDRIVE_MODE, "abort", &freedrive_mode_abort_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", &passthrough_trajectory_transfer_state_)); @@ -849,52 +850,52 @@ void URPositionHardwareInterface::checkAsyncIO() 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]); + // 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]); tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_); tcp_torque_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_torque_); -urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), + 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)); + // 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]); if (tcp_angle > 1e-16) { @@ -960,48 +961,6 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } } } - // set new mode to all interfaces at the same time - if (start_modes_.size() != 0 && start_modes_.size() != 6) { - 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 interface or freedrive mode 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 interface or freedrive mode 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; - } // Stopping interfaces // add stop interface per joint in tmp var for later check From 673df98cc9ee3c20bc0533737e501389da0bb319 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 18 Nov 2024 09:46:00 +0000 Subject: [PATCH 29/54] Adapted freedrive to new command_switch --- ur_robot_driver/src/hardware_interface.cpp | 29 +++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index a0038e129..bd4f6464c 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -927,6 +927,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (passthrough_trajectory_controller_running_) { control_modes[i] = PASSTHROUGH_GPIO; } + if (freedrive_mode_controller_running_) { + control_modes[i] = FREEDRIVE_MODE; + } } if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), @@ -955,9 +958,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i] = PASSTHROUGH_GPIO; - } - if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { - start_modes_.push_back(FREEDRIVE_MODE); + } else if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = FREEDRIVE_MODE; } } } @@ -984,6 +989,12 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod control_modes[i] = "UNDEFINED"; } } + if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { + stop_modes_.push_back(StoppingInterface::STOP_FREEDRIVE); + if (control_modes[i] == FREEDRIVE_MODE) { + control_modes[i] = "UNDEFINED"; + } + } } } @@ -1000,6 +1011,18 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod }))) { ret_val = hardware_interface::return_type::ERROR; } + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == FREEDRIVE_MODE); }) && + (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO); + }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO || item == FREEDRIVE_MODE); + }))) { + ret_val = hardware_interface::return_type::ERROR; + } if (std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && (std::any_of( From 6eda9a5b32701d91e295793c091c4935b382742f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 18 Nov 2024 09:46:38 +0000 Subject: [PATCH 30/54] First of version of the topic-based implementation --- .../freedrive_mode_controller.hpp | 4 +++ .../src/freedrive_mode_controller.cpp | 34 +++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index eeb235a34..3f62ff1a4 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -52,6 +52,7 @@ #include #include #include +#include "std_msgs/msg/bool.hpp" #include "freedrive_mode_controller_parameters.hpp" @@ -93,9 +94,11 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::optional> enable_command_interface_; std::optional> abort_command_interface_; + std::shared_ptr> enable_freedrive_mode_sub_; rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); std::shared_ptr freedrive_param_listener_; freedrive_mode_controller::Params freedrive_params_; @@ -103,6 +106,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::atomic freedrive_active_; std::atomic change_requested_; std::atomic async_state_; + std::atomic first_log_; 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 1a8d83646..4e073cb1a 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -83,6 +83,10 @@ controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) { + // Subscriber definition + enable_freedrive_mode_sub_ = get_node()->create_subscription( + "~/freedrive_mode_active", 10, + std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); const auto logger = get_node()->get_logger(); @@ -105,6 +109,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { change_requested_ = false; freedrive_active_ = false; + first_log_ = false; async_state_ = std::numeric_limits::quiet_NaN(); { @@ -155,6 +160,11 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S { abort_command_interface_->get().set_value(1.0); + // Set enable value to false, so in the update + // we can deactivate the freedrive mode + //Old comment? + freedrive_active_ = false; + return CallbackReturn::SUCCESS; } @@ -187,13 +197,37 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat async_success_command_interface_->get().set_value(ASYNC_WAITING); async_state_ = ASYNC_WAITING; } + first_log_ = true; change_requested_ = false; } + + if((async_state_ == 1.0) && (first_log_)){ + if(freedrive_active_){ + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } + first_log_ = false; + } return controller_interface::return_type::OK; } +void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { // Process the freedrive_mode command. + if(msg->data) + { + RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); + if((!freedrive_active_) && (!change_requested_)){ + freedrive_active_ = true; + change_requested_ = true; + } + } else{ + RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); + if((freedrive_active_) && (!change_requested_)){ + freedrive_active_ = false; + change_requested_ = true; + } } } From d02513c31af0d31c4ec68e4c491c3c268c50cca0 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 18 Nov 2024 09:51:14 +0000 Subject: [PATCH 31/54] Fix format --- ur_controllers/src/freedrive_mode_controller.cpp | 8 ++++---- .../src/freedrive_mode_controller_parameters.yaml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 4e073cb1a..0caa79408 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -84,7 +84,7 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St { // Subscriber definition - enable_freedrive_mode_sub_ = get_node()->create_subscription( + enable_freedrive_mode_sub_ = get_node()->create_subscription( "~/freedrive_mode_active", 10, std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); @@ -186,7 +186,7 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat // 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; } @@ -237,7 +237,7 @@ 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) { @@ -331,4 +331,4 @@ bool FreedriveModeController::waitForAsyncCommand(std::function ge #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/freedrive_mode_controller_parameters.yaml b/ur_controllers/src/freedrive_mode_controller_parameters.yaml index cd58c0cf5..ee4915e49 100644 --- a/ur_controllers/src/freedrive_mode_controller_parameters.yaml +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -9,4 +9,4 @@ freedrive_mode_controller: type: int, default_value: 10, description: "Amount of retries for checking if setting force_mode was successful" - } \ No newline at end of file + } From 22c1a057b4bef4949e5b1fead2298db7b5773270 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 18 Nov 2024 17:25:26 +0000 Subject: [PATCH 32/54] Fixed print location for received commands --- ur_controllers/src/freedrive_mode_controller.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 0caa79408..723133915 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -184,6 +184,8 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat return controller_interface::return_type::OK; } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); + // Set command interface to enable enable_command_interface_->get().set_value(1.0); @@ -192,6 +194,8 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat } } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); + abort_command_interface_->get().set_value(1.0); async_success_command_interface_->get().set_value(ASYNC_WAITING); @@ -217,13 +221,11 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh // Process the freedrive_mode command. if(msg->data) { - RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); if((!freedrive_active_) && (!change_requested_)){ freedrive_active_ = true; change_requested_ = true; } } else{ - RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); if((freedrive_active_) && (!change_requested_)){ freedrive_active_ = false; change_requested_ = true; From 3d906ddb0e12942d3b5330509c1f52a15057593f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 19 Nov 2024 06:15:33 +0000 Subject: [PATCH 33/54] First version with timer --- .../freedrive_mode_controller.hpp | 7 +++-- .../src/freedrive_mode_controller.cpp | 31 +++++++++++++++++++ 2 files changed, 36 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 3f62ff1a4..9a6147c09 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -96,8 +96,8 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::shared_ptr> enable_freedrive_mode_sub_; - rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal - rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check on the topic + std::chrono::seconds timeout_interval_ = std::chrono::seconds(2); void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); std::shared_ptr freedrive_param_listener_; @@ -107,6 +107,9 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::atomic change_requested_; std::atomic async_state_; std::atomic first_log_; + std::atomic timer_started_; + + void timeout_callback(); 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 723133915..b198f7ce8 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -88,6 +88,8 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St "~/freedrive_mode_active", 10, std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); + timer_started_ = false; + const auto logger = get_node()->get_logger(); if (!freedrive_param_listener_) { @@ -218,6 +220,17 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { + if (!timer_started_) + { + // Start the timer only after the first message is received + freedrive_sub_timer_ = get_node()->create_wall_timer( + timeout_interval_, + std::bind(&FreedriveModeController::timeout_callback, this)); + timer_started_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); + } + // Process the freedrive_mode command. if(msg->data) { @@ -231,6 +244,24 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh change_requested_ = true; } } + + if (freedrive_sub_timer_) + { + freedrive_sub_timer_->reset(); + } +} + +void FreedriveModeController::timeout_callback() +{ + + if(timer_started_){ + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since client is not reachable."); + + freedrive_active_ = false; + change_requested_ = true; + } + + timer_started_ = false; } // Timeout handling for the topic From b7efaa492eff93128009c962c58677e5fa11eb51 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 19 Nov 2024 06:26:47 +0000 Subject: [PATCH 34/54] Fix to start timer only for relevant msgs --- .../freedrive_mode_controller.hpp | 1 + .../src/freedrive_mode_controller.cpp | 26 ++++++++++++------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 9a6147c09..aa58bbc3c 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -109,6 +109,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::atomic first_log_; std::atomic timer_started_; + void start_timer(); void timeout_callback(); 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 b198f7ce8..aef56289f 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -220,16 +220,6 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { - if (!timer_started_) - { - // Start the timer only after the first message is received - freedrive_sub_timer_ = get_node()->create_wall_timer( - timeout_interval_, - std::bind(&FreedriveModeController::timeout_callback, this)); - timer_started_ = true; - - RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); - } // Process the freedrive_mode command. if(msg->data) @@ -237,11 +227,13 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh if((!freedrive_active_) && (!change_requested_)){ freedrive_active_ = true; change_requested_ = true; + start_timer(); } } else{ if((freedrive_active_) && (!change_requested_)){ freedrive_active_ = false; change_requested_ = true; + start_timer(); } } @@ -251,6 +243,20 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh } } +void FreedriveModeController::start_timer() +{ + if (!timer_started_) + { + // Start the timer only after the first message is received + freedrive_sub_timer_ = get_node()->create_wall_timer( + timeout_interval_, + std::bind(&FreedriveModeController::timeout_callback, this)); + timer_started_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); + } +} + void FreedriveModeController::timeout_callback() { From e26951837b5e8e363815d7a942791cf1bca20b92 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Wed, 20 Nov 2024 13:24:47 +0000 Subject: [PATCH 35/54] Fix Format --- .../freedrive_mode_controller.hpp | 4 ++-- .../src/freedrive_mode_controller.cpp | 13 +++++-------- ur_robot_driver/src/hardware_interface.cpp | 18 ++++++++++-------- 3 files changed, 17 insertions(+), 18 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index aa58bbc3c..72596b896 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -119,5 +119,5 @@ class FreedriveModeController : public controller_interface::ControllerInterface */ bool waitForAsyncCommand(std::function get_value); }; -} // namespace ur_controllers -#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index aef56289f..c444f4b49 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -71,7 +71,8 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in return config; } -controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeController::state_interface_configuration() const +controller_interface::InterfaceConfiguration +ur_controllers::FreedriveModeController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::NONE; @@ -82,7 +83,6 @@ controller_interface::InterfaceConfiguration ur_controllers::FreedriveModeContro controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) { - // Subscriber definition enable_freedrive_mode_sub_ = get_node()->create_subscription( "~/freedrive_mode_active", 10, @@ -164,7 +164,7 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S // Set enable value to false, so in the update // we can deactivate the freedrive mode - //Old comment? + // Old comment? freedrive_active_ = false; return CallbackReturn::SUCCESS; @@ -177,15 +177,14 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat if(change_requested_) { if (freedrive_active_) { - // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the teach - // pendant. + // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the + // teach pendant. if (!std::isnan(abort_command_interface_->get().get_value()) && abort_command_interface_->get().get_value() == 1.0) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action."); freedrive_active_ = false; return controller_interface::return_type::OK; } else { - RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); // Set command interface to enable @@ -220,7 +219,6 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { - // Process the freedrive_mode command. if(msg->data) { @@ -259,7 +257,6 @@ void FreedriveModeController::start_timer() void FreedriveModeController::timeout_callback() { - if(timer_started_){ RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since client is not reachable."); diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index bd4f6464c..bf90ed806 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -834,7 +834,8 @@ void URPositionHardwareInterface::checkAsyncIO() if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { RCLCPP_INFO(get_logger(), "Starting freedrive mode."); - freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); freedrive_mode_enable_ = NO_NEW_CMD_; freedrive_action_requested_ = true; } @@ -842,7 +843,8 @@ void URPositionHardwareInterface::checkAsyncIO() 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_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); freedrive_action_requested_ = false; freedrive_mode_abort_ = NO_NEW_CMD_; } @@ -882,13 +884,13 @@ void URPositionHardwareInterface::transformForceTorque() 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]); + urcl_ft_sensor_measurements_[5]); tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_); tcp_torque_ = tf2::quatRotate(tcp_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() }; + tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; } void URPositionHardwareInterface::extractToolPose() @@ -907,7 +909,7 @@ void URPositionHardwareInterface::extractToolPose() } 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; @@ -1011,11 +1013,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod }))) { ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), - [this](auto& item) { return (item == FREEDRIVE_MODE); }) && + if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FREEDRIVE_MODE); }) && (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO); + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); }) || std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || From d12cbdfe648334beda1843e6aed2888ac24e6e14 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sun, 24 Nov 2024 10:13:51 +0000 Subject: [PATCH 36/54] Cleanup and more name convention --- .../freedrive_mode_controller.hpp | 13 ++- .../src/freedrive_mode_controller.cpp | 85 +------------------ .../ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 22 ++--- 4 files changed, 18 insertions(+), 104 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 72596b896..4301cfb2a 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -29,8 +29,8 @@ //---------------------------------------------------------------------- /*!\file * - * \author Felix Exner exner@fzi.de - * \date 2023-06-29 + * \author Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-26 */ //---------------------------------------------------------------------- #ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ @@ -60,12 +60,9 @@ namespace ur_controllers { enum CommandInterfaces { - FREEDRIVE_MODE_ASYNC_SUCCESS = 25, - FREEDRIVE_MODE_CMD = 26, -}; -enum StateInterfaces -{ - INITIALIZED_FLAG = 0u, + FREEDRIVE_MODE_ASYNC_SUCCESS = 0u, + FREEDRIVE_MODE_ENABLE = 1, + FREEDRIVE_MODE_ABORT = 2, }; using namespace std::chrono_literals; // NOLINT diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index c444f4b49..2c0970350 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -85,7 +85,7 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St { // Subscriber definition enable_freedrive_mode_sub_ = get_node()->create_subscription( - "~/freedrive_mode_active", 10, + "~/enable_freedrive_mode", 10, std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); timer_started_ = false; @@ -267,89 +267,6 @@ void FreedriveModeController::timeout_callback() timer_started_ = false; } -// Timeout handling for the topic -/* -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."); - - freedrive_active_ = false; - change_requested_ = true; - - 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)); - } - - success = async_state_ == 1.0; - 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); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - return rclcpp_action::CancelResponse::ACCEPT; -} -*/ - -// Don't need this anymore, but logic must be reproduced in subscriber topic -/* -void FreedriveModeController::goal_accepted_callback( - std::shared_ptr> goal_handle) -{ - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); - - bool success; - - freedrive_active_ = true; - change_requested_ = true; - - 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_) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - retries++; - - if (retries > maximum_retries) { - success = false; - } - } - - // Check if the change was successful - success = async_state_ == 1.0; - - if (success) { - 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."); - } - - // 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)); - return; -} -*/ - bool FreedriveModeController::waitForAsyncCommand(std::function get_value) { const auto maximum_retries = freedrive_params_.check_io_successful_retries; 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 2742c846f..888acb4c1 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -287,7 +287,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"; + const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index bf90ed806..c3fd7feb3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -341,13 +341,13 @@ 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, "async_success", &freedrive_mode_async_success_)); + tf_prefix + FREEDRIVE_MODE_GPIO, "async_success", &freedrive_mode_async_success_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + FREEDRIVE_MODE, "enable", &freedrive_mode_enable_)); + tf_prefix + FREEDRIVE_MODE_GPIO, "enable", &freedrive_mode_enable_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + FREEDRIVE_MODE, "abort", &freedrive_mode_abort_)); + tf_prefix + FREEDRIVE_MODE_GPIO, "abort", &freedrive_mode_abort_)); command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", &passthrough_trajectory_transfer_state_)); @@ -930,7 +930,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod control_modes[i] = PASSTHROUGH_GPIO; } if (freedrive_mode_controller_running_) { - control_modes[i] = FREEDRIVE_MODE; + control_modes[i] = FREEDRIVE_MODE_GPIO; } } @@ -960,11 +960,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i] = PASSTHROUGH_GPIO; - } else if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { + } else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { if (start_modes_[i] != "UNDEFINED") { return hardware_interface::return_type::ERROR; } - start_modes_[i] = FREEDRIVE_MODE; + start_modes_[i] = FREEDRIVE_MODE_GPIO; } } } @@ -991,9 +991,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod control_modes[i] = "UNDEFINED"; } } - if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { + if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { stop_modes_.push_back(StoppingInterface::STOP_FREEDRIVE); - if (control_modes[i] == FREEDRIVE_MODE) { + if (control_modes[i] == FREEDRIVE_MODE_GPIO) { control_modes[i] = "UNDEFINED"; } } @@ -1013,7 +1013,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod }))) { ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FREEDRIVE_MODE); }) && + if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || @@ -1021,7 +1021,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod }) || std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO || item == FREEDRIVE_MODE); + item == PASSTHROUGH_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { ret_val = hardware_interface::return_type::ERROR; } @@ -1090,7 +1090,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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()) { + std::find(start_modes_.begin(), start_modes_.end(), FREEDRIVE_MODE_GPIO) != start_modes_.end()) { velocity_controller_running_ = false; position_controller_running_ = false; freedrive_mode_controller_running_ = true; From 29761819f388a406310d7f22ee88101c484fb78f Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 25 Nov 2024 06:10:15 +0000 Subject: [PATCH 37/54] Proper handling of controller conflicts --- ur_robot_driver/src/hardware_interface.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index c3fd7feb3..3e50d7679 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -1004,16 +1004,18 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && (std::any_of(start_modes_.begin(), start_modes_.end(), - [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); }) || std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == FREEDRIVE_MODE_GPIO || item == PASSTHROUGH_GPIO); }))) { ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || @@ -1027,12 +1029,14 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } if (std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && - (std::any_of( - start_modes_.begin(), start_modes_.end(), - [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) || + (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == FREEDRIVE_MODE_GPIO || + item == PASSTHROUGH_GPIO); + }) || std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == FREEDRIVE_MODE_GPIO || item == PASSTHROUGH_GPIO); }))) { ret_val = hardware_interface::return_type::ERROR; } @@ -1040,7 +1044,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == FREEDRIVE_MODE_GPIO || item == PASSTHROUGH_GPIO); })) { ret_val = hardware_interface::return_type::ERROR; } From 920e60f859c79cad6b82b19b6455de4909a6aaaf Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 25 Nov 2024 07:35:49 +0000 Subject: [PATCH 38/54] Updated variable name to meaningful one --- .../include/ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 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 888acb4c1..2aeae8d00 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -227,7 +227,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_build_; // Freedrive mode controller interface values - bool freedrive_action_requested_; + bool freedrive_activated_; bool freedrive_mode_controller_running_; double freedrive_mode_async_success_; double freedrive_mode_enable_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3e50d7679..859f33f22 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -721,7 +721,7 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); - } else if (freedrive_mode_controller_running_ && freedrive_action_requested_) { + } else if (freedrive_mode_controller_running_ && freedrive_activated_) { ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); } else if (passthrough_trajectory_controller_running_) { @@ -837,15 +837,15 @@ void URPositionHardwareInterface::checkAsyncIO() freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); freedrive_mode_enable_ = NO_NEW_CMD_; - freedrive_action_requested_ = true; + freedrive_activated_ = true; } - if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_action_requested_ && + if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_activated_ && ur_driver_ != nullptr) { RCLCPP_INFO(get_logger(), "Stopping freedrive mode."); freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); - freedrive_action_requested_ = false; + freedrive_activated_ = false; freedrive_mode_abort_ = NO_NEW_CMD_; } } @@ -1069,7 +1069,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_activated_ = false; freedrive_mode_abort_ = 1.0; } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { @@ -1098,7 +1098,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod velocity_controller_running_ = false; position_controller_running_ = false; freedrive_mode_controller_running_ = true; - freedrive_action_requested_ = false; + freedrive_activated_ = false; } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) { velocity_controller_running_ = false; From 5a4ecfff085e224c8e4ea859f5373f600fde7da5 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 25 Nov 2024 07:36:42 +0000 Subject: [PATCH 39/54] Fixed timer issue: starting only if active and for enable msgs --- ur_controllers/src/freedrive_mode_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 2c0970350..9ea15e54e 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -181,7 +181,7 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat // teach pendant. if (!std::isnan(abort_command_interface_->get().get_value()) && abort_command_interface_->get().get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action."); + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request."); freedrive_active_ = false; return controller_interface::return_type::OK; } else { @@ -231,7 +231,7 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh if((freedrive_active_) && (!change_requested_)){ freedrive_active_ = false; change_requested_ = true; - start_timer(); + //start_timer(); } } @@ -257,7 +257,7 @@ void FreedriveModeController::start_timer() void FreedriveModeController::timeout_callback() { - if(timer_started_){ + if(timer_started_ && freedrive_active_){ RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since client is not reachable."); freedrive_active_ = false; From c388e4a7a6df9975c62fddd80aab1e7b20529296 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Thu, 5 Dec 2024 12:45:15 +0000 Subject: [PATCH 40/54] Added initial documentation --- ur_controllers/doc/index.rst | 28 +++++++++++++++++++++++ ur_robot_driver/doc/usage/controllers.rst | 7 ++++++ 2 files changed, 35 insertions(+) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index a488d07e5..c4deb7069 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -264,3 +264,31 @@ Implementation details / dataflow * When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort`` command interface), the action will be aborted. * When the action is preempted, execution on the hardware is preempted. + +.. _freedrive_mode_controller: + +ur_controllers/FreedriveModeController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller activates the robot's *Freedrive Mode*, allowing to freely manually move the robot. +For such reason, the controller is never (and can't be) coupled with any other controller. + +Parameters +"""""""""" + ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ + +Usage +""""" + +The controller provides the ``enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: + +* to start and keep the freedrive active, it is necessary to continously publish a ``True`` msg on the indicated topic. + To deal with malfunctioning on the client side, the controller deactivates the freedrive mode if messages + are not received anymore within the ``timeout`` parameter. ++ to deactivate the freedrive mode is enough to publish a ``False`` msg on the indicated topic or to deactivate the controller. \ No newline at end of file diff --git a/ur_robot_driver/doc/usage/controllers.rst b/ur_robot_driver/doc/usage/controllers.rst index d1566758b..2a4281744 100644 --- a/ur_robot_driver/doc/usage/controllers.rst +++ b/ur_robot_driver/doc/usage/controllers.rst @@ -110,3 +110,10 @@ Type: `position_controllers/JointGroupPositionController ` + +Allows utilizing the robot's *Freedrive mode*, making possible to manually move the robot in a desired configuration. \ No newline at end of file From 4b834c001255275c4561b8ca8d24d6e22aadd5a9 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sat, 7 Dec 2024 10:53:12 +0000 Subject: [PATCH 41/54] Added inactive_timeout as parameter --- ur_controllers/doc/index.rst | 6 ++++-- .../include/ur_controllers/freedrive_mode_controller.hpp | 2 +- ur_controllers/src/freedrive_mode_controller.cpp | 1 + .../src/freedrive_mode_controller_parameters.yaml | 5 +++++ 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index c4deb7069..839174403 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -282,13 +282,15 @@ Parameters +----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ | ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | +----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``inactive_timeout`` | int | 10 | Time interval (in seconds) of message inactivity after which freedrive is deactivated | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ Usage """"" -The controller provides the ``enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: +The controller provides the ``/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: * to start and keep the freedrive active, it is necessary to continously publish a ``True`` msg on the indicated topic. To deal with malfunctioning on the client side, the controller deactivates the freedrive mode if messages - are not received anymore within the ``timeout`` parameter. + are not received anymore within the ``inactive_timeout`` parameter. + to deactivate the freedrive mode is enough to publish a ``False`` msg on the indicated topic or to deactivate the controller. \ No newline at end of file diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 4301cfb2a..d9470549f 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -94,7 +94,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::shared_ptr> enable_freedrive_mode_sub_; rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check on the topic - std::chrono::seconds timeout_interval_ = std::chrono::seconds(2); + mutable std::chrono::seconds timeout_interval_; void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); std::shared_ptr freedrive_param_listener_; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 9ea15e54e..ce37f96a3 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -62,6 +62,7 @@ controller_interface::InterfaceConfiguration FreedriveModeController::command_in config.type = controller_interface::interface_configuration_type::INDIVIDUAL; const std::string tf_prefix = freedrive_params_.tf_prefix; + timeout_interval_ = std::chrono::seconds(freedrive_params_.inactive_timeout); // Get the command interfaces needed for freedrive mode from the hardware interface config.names.emplace_back(tf_prefix + "freedrive_mode/async_success"); diff --git a/ur_controllers/src/freedrive_mode_controller_parameters.yaml b/ur_controllers/src/freedrive_mode_controller_parameters.yaml index ee4915e49..6dc105782 100644 --- a/ur_controllers/src/freedrive_mode_controller_parameters.yaml +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -5,6 +5,11 @@ freedrive_mode_controller: default_value: "", description: "Urdf prefix of the corresponding arm" } + inactive_timeout: { + type: int, + default_value: 10, + description: "Time interval (in seconds) of message inactivity after which freedrive is deactivated" + } check_io_successful_retries: { type: int, default_value: 10, From 6869b567d3714864b1d7184615f72134f49f682c Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sun, 8 Dec 2024 22:49:03 +0000 Subject: [PATCH 42/54] Added async thread to log outside of update() --- .../freedrive_mode_controller.hpp | 14 +++- .../src/freedrive_mode_controller.cpp | 64 +++++++++++++++---- 2 files changed, 64 insertions(+), 14 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index d9470549f..502be4091 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -38,12 +38,11 @@ #pragma once -#include -#include - #include #include #include +#include +#include #include #include @@ -109,6 +108,15 @@ class FreedriveModeController : public controller_interface::ControllerInterface void start_timer(); void timeout_callback(); + std::thread logging_thread_; + std::atomic logging_thread_running_; + std::atomic logging_requested_; + std::condition_variable logging_condition_; + std::mutex log_mutex_; + void log_task(); + void start_logging_thread(); + void stop_logging_thread(); + static constexpr double ASYNC_WAITING = 2.0; /** * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index ce37f96a3..0766adba1 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -104,6 +104,8 @@ 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(); + start_logging_thread(); + return ControllerInterface::on_configure(previous_state); } @@ -112,9 +114,12 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { change_requested_ = false; freedrive_active_ = false; - first_log_ = false; async_state_ = std::numeric_limits::quiet_NaN(); + first_log_ = false; + logging_thread_running_ = false; + logging_requested_ = false; + { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" "async_success"; @@ -162,12 +167,10 @@ controller_interface::CallbackReturn ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) { abort_command_interface_->get().set_value(1.0); - - // Set enable value to false, so in the update - // we can deactivate the freedrive mode - // Old comment? freedrive_active_ = false; + stop_logging_thread(); + return CallbackReturn::SUCCESS; } @@ -208,12 +211,12 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat } if((async_state_ == 1.0) && (first_log_)){ - if(freedrive_active_){ - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); - } else { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); - } first_log_ = false; + logging_requested_= true; + + // Notify logging thread + logging_condition_.notify_one(); + } return controller_interface::return_type::OK; } @@ -232,7 +235,6 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh if((freedrive_active_) && (!change_requested_)){ freedrive_active_ = false; change_requested_ = true; - //start_timer(); } } @@ -268,6 +270,46 @@ void FreedriveModeController::timeout_callback() timer_started_ = false; } + void FreedriveModeController::start_logging_thread() { + if (!logging_thread_running_) { + logging_thread_running_ = true; + logging_thread_ = std::thread(&FreedriveModeController::log_task, this); + } + } + + void FreedriveModeController::stop_logging_thread() { + logging_thread_running_ = false; + if (logging_thread_.joinable()) { + logging_thread_.join(); + } + } + + void FreedriveModeController::log_task() { + while (logging_thread_running_) { + + std::unique_lock lock(log_mutex_); + + auto condition = [this] { + return !logging_thread_running_ || logging_requested_; + }; + + // Wait for the condition + logging_condition_.wait(lock, condition); + + + if (!logging_thread_running_) break; + + if(freedrive_active_){ + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } + + // Reset to log only once + logging_requested_ = false; + } + } + bool FreedriveModeController::waitForAsyncCommand(std::function get_value) { const auto maximum_retries = freedrive_params_.check_io_successful_retries; From 406503575cf956b24e8204293731e66ecea328d1 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sun, 8 Dec 2024 22:49:16 +0000 Subject: [PATCH 43/54] Format docs --- ur_controllers/doc/index.rst | 4 ++-- ur_robot_driver/doc/usage/controllers.rst | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 839174403..36c09f128 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -290,7 +290,7 @@ Usage The controller provides the ``/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: -* to start and keep the freedrive active, it is necessary to continously publish a ``True`` msg on the indicated topic. +* to start and keep the freedrive active, it is necessary to continuously publish a ``True`` msg on the indicated topic. To deal with malfunctioning on the client side, the controller deactivates the freedrive mode if messages are not received anymore within the ``inactive_timeout`` parameter. -+ to deactivate the freedrive mode is enough to publish a ``False`` msg on the indicated topic or to deactivate the controller. \ No newline at end of file ++ to deactivate the freedrive mode is enough to publish a ``False`` msg on the indicated topic or to deactivate the controller. diff --git a/ur_robot_driver/doc/usage/controllers.rst b/ur_robot_driver/doc/usage/controllers.rst index 2a4281744..64dca4230 100644 --- a/ur_robot_driver/doc/usage/controllers.rst +++ b/ur_robot_driver/doc/usage/controllers.rst @@ -116,4 +116,4 @@ freedrive_mode_controller Type: :ref `ur_controllers/FreedriveModeController ` -Allows utilizing the robot's *Freedrive mode*, making possible to manually move the robot in a desired configuration. \ No newline at end of file +Allows utilizing the robot's *Freedrive mode*, making possible to manually move the robot in a desired configuration. From 82328ca7bdb916f5bf603b496f8b8854e3260ad3 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Sun, 8 Dec 2024 23:45:23 +0000 Subject: [PATCH 44/54] Added check on active condition --- .../src/freedrive_mode_controller.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 0766adba1..2bf7b880d 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -169,6 +169,9 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S abort_command_interface_->get().set_value(1.0); freedrive_active_ = false; + freedrive_sub_timer_.reset(); + timer_started_ = false; + stop_logging_thread(); return CallbackReturn::SUCCESS; @@ -224,17 +227,20 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { // Process the freedrive_mode command. - if(msg->data) + if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - if((!freedrive_active_) && (!change_requested_)){ - freedrive_active_ = true; - change_requested_ = true; - start_timer(); - } - } else{ - if((freedrive_active_) && (!change_requested_)){ - freedrive_active_ = false; - change_requested_ = true; + if(msg->data) + { + if((!freedrive_active_) && (!change_requested_)){ + freedrive_active_ = true; + change_requested_ = true; + start_timer(); + } + } else{ + if((freedrive_active_) && (!change_requested_)){ + freedrive_active_ = false; + change_requested_ = true; + } } } From b965673d1441fd5e612e6e8530017ed89deeb920 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 9 Dec 2024 07:45:58 +0000 Subject: [PATCH 45/54] Initial tests --- ur_controllers/CMakeLists.txt | 19 ++++++ .../freedrive_mode_controller_params.yaml | 5 ++ .../test_load_freedrive_mode_controller.cpp | 60 +++++++++++++++++++ .../integration_test_controller_switch.py | 20 +++++++ 4 files changed, 104 insertions(+) create mode 100644 ur_controllers/test/freedrive_mode_controller_params.yaml create mode 100644 ur_controllers/test/test_load_freedrive_mode_controller.cpp diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a494a3bfc..ef7197f56 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -131,4 +131,23 @@ ament_export_libraries( ${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_freedrive_mode_controller + test/test_load_freedrive_mode_controller.cpp + ) + target_link_libraries(test_load_freedrive_mode_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_load_freedrive_mode_controller + controller_manager + ros2_control_test_assets + ) +endif() + ament_package() diff --git a/ur_controllers/test/freedrive_mode_controller_params.yaml b/ur_controllers/test/freedrive_mode_controller_params.yaml new file mode 100644 index 000000000..417d385c0 --- /dev/null +++ b/ur_controllers/test/freedrive_mode_controller_params.yaml @@ -0,0 +1,5 @@ +--- +freedrive_mode_controller: + ros__parameters: + tf_prefix: "" + inactive_timeout: 10 diff --git a/ur_controllers/test/test_load_freedrive_mode_controller.cpp b/ur_controllers/test/test_load_freedrive_mode_controller.cpp new file mode 100644 index 000000000..cf3ce4292 --- /dev/null +++ b/ur_controllers/test/test_load_freedrive_mode_controller.cpp @@ -0,0 +1,60 @@ +// Copyright 2024, Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadFreedriveModeController, load_controller) +{ + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm{ executor, ros2_control_test_assets::minimal_robot_urdf, true, + "test_controller_manager" }; + + const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/freedrive_mode_controller_params.yaml"; + cm.set_parameter({ "test_freedrive_mode_controller.params_file", test_file_path }); + + cm.set_parameter({ "test_freedrive_mode_controller.type", "ur_controllers/FreedriveModeController" }); + + ASSERT_NE(cm.load_controller("test_freedrive_mode_controller"), nullptr); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 20ea69190..75659958b 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -94,6 +94,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): "forward_position_controller", "forward_velocity_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ], ).ok ) @@ -138,6 +139,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): "forward_position_controller", "forward_velocity_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ], ).ok ) @@ -177,6 +179,15 @@ def test_activating_multiple_controllers_different_interface_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "freedrive_mode_controller", + ], + ).ok + ) def test_activating_controller_with_running_position_controller_fails(self): # Having a position-based controller active, no other controller should be able to @@ -191,6 +202,7 @@ def test_activating_controller_with_running_position_controller_fails(self): "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -211,6 +223,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) self.assertFalse( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.STRICT, From 67f225664efc5981b01d36c4a6c94d17a4e2962d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 13 Dec 2024 15:03:13 +0100 Subject: [PATCH 46/54] Fix formatting --- .../freedrive_mode_controller.hpp | 2 +- .../src/freedrive_mode_controller.cpp | 98 +++++++++---------- 2 files changed, 47 insertions(+), 53 deletions(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 502be4091..ca66811d1 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -64,7 +64,7 @@ enum CommandInterfaces FREEDRIVE_MODE_ABORT = 2, }; -using namespace std::chrono_literals; // NOLINT +using namespace std::chrono_literals; // NOLINT class FreedriveModeController : public controller_interface::ControllerInterface { diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 2bf7b880d..f5ba423d3 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -122,7 +122,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" - "async_success"; + "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()) { @@ -135,7 +135,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" - "enable"; + "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()) { @@ -148,7 +148,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta { const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" - "abort"; + "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()) { @@ -178,11 +178,11 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S } controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& /*period*/) + const rclcpp::Duration& /*period*/) { async_state_ = async_success_command_interface_->get().get_value(); - if(change_requested_) { + if (change_requested_) { if (freedrive_active_) { // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the // teach pendant. @@ -213,13 +213,12 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat change_requested_ = false; } - if((async_state_ == 1.0) && (first_log_)){ + if ((async_state_ == 1.0) && (first_log_)) { first_log_ = false; - logging_requested_= true; + logging_requested_ = true; // Notify logging thread logging_condition_.notify_one(); - } return controller_interface::return_type::OK; } @@ -227,37 +226,32 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) { // Process the freedrive_mode command. - if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - if(msg->data) - { - if((!freedrive_active_) && (!change_requested_)){ + if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (msg->data) { + if ((!freedrive_active_) && (!change_requested_)) { freedrive_active_ = true; change_requested_ = true; start_timer(); } - } else{ - if((freedrive_active_) && (!change_requested_)){ + } else { + if ((freedrive_active_) && (!change_requested_)) { freedrive_active_ = false; change_requested_ = true; } } } - if (freedrive_sub_timer_) - { + if (freedrive_sub_timer_) { freedrive_sub_timer_->reset(); } } void FreedriveModeController::start_timer() { - if (!timer_started_) - { + if (!timer_started_) { // Start the timer only after the first message is received - freedrive_sub_timer_ = get_node()->create_wall_timer( - timeout_interval_, - std::bind(&FreedriveModeController::timeout_callback, this)); + freedrive_sub_timer_ = + get_node()->create_wall_timer(timeout_interval_, std::bind(&FreedriveModeController::timeout_callback, this)); timer_started_ = true; RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); @@ -266,7 +260,7 @@ void FreedriveModeController::start_timer() void FreedriveModeController::timeout_callback() { - if(timer_started_ && freedrive_active_){ + if (timer_started_ && freedrive_active_) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since client is not reachable."); freedrive_active_ = false; @@ -276,45 +270,45 @@ void FreedriveModeController::timeout_callback() timer_started_ = false; } - void FreedriveModeController::start_logging_thread() { - if (!logging_thread_running_) { - logging_thread_running_ = true; - logging_thread_ = std::thread(&FreedriveModeController::log_task, this); - } +void FreedriveModeController::start_logging_thread() +{ + if (!logging_thread_running_) { + logging_thread_running_ = true; + logging_thread_ = std::thread(&FreedriveModeController::log_task, this); } +} - void FreedriveModeController::stop_logging_thread() { - logging_thread_running_ = false; - if (logging_thread_.joinable()) { - logging_thread_.join(); - } +void FreedriveModeController::stop_logging_thread() +{ + logging_thread_running_ = false; + if (logging_thread_.joinable()) { + logging_thread_.join(); } +} - void FreedriveModeController::log_task() { - while (logging_thread_running_) { - - std::unique_lock lock(log_mutex_); - - auto condition = [this] { - return !logging_thread_running_ || logging_requested_; - }; +void FreedriveModeController::log_task() +{ + while (logging_thread_running_) { + std::unique_lock lock(log_mutex_); - // Wait for the condition - logging_condition_.wait(lock, condition); + auto condition = [this] { return !logging_thread_running_ || logging_requested_; }; + // Wait for the condition + logging_condition_.wait(lock, condition); - if (!logging_thread_running_) break; + if (!logging_thread_running_) + break; - if(freedrive_active_){ - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); - } else { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); - } - - // Reset to log only once - logging_requested_ = false; + if (freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); } + + // Reset to log only once + logging_requested_ = false; } +} bool FreedriveModeController::waitForAsyncCommand(std::function get_value) { From 8e85c2a10f797c4805fc8dbb06e60bfe8a943616 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 13 Dec 2024 15:36:46 +0100 Subject: [PATCH 47/54] Fix resource claiming in hw interface --- ur_robot_driver/src/hardware_interface.cpp | 26 ++++++++++++++++--- .../integration_test_controller_switch.py | 16 ++++++++++++ 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index a54bb4a79..536803bb6 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -1076,6 +1076,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } // Do not start conflicting controllers + // Passthrough controller requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), @@ -1092,15 +1093,34 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod ret_val = hardware_interface::return_type::ERROR; } + // Force mode requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == FORCE_MODE_GPIO); }) && (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), - [](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); + }))) { + RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or " + "velocity mode running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // Freedrive mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); }) || std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == FORCE_MODE_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); }))) { RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or " "velocity mode running."); diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index cd5a03c3e..a9356ffc7 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -179,6 +179,14 @@ def test_activating_multiple_controllers_different_interface_fails(self): activate_controllers=[ "scaled_joint_trajectory_controller", "force_mode_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", "freedrive_mode_controller", ], ).ok @@ -301,6 +309,14 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) # Stop the controller again self.assertTrue( self._controller_manager_interface.switch_controller( From 5a8ef90b812d67fdfa496364c86a6dc8d5fdfd35 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 16 Dec 2024 09:18:25 +0100 Subject: [PATCH 48/54] Update ur_controllers/doc/index.rst Co-authored-by: Felix Exner --- ur_controllers/doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 69607441b..f03632d53 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -364,7 +364,7 @@ Parameters Usage """"" -The controller provides the ``/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: +The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: * to start and keep freedrive active, you'll have to frequently publish a ``True`` msg on the indicated topic. If no further messages are received by the controller within the ``inactive_timeout`` seconds, From 3eab80cd4833ed1282fc3a9b8d91d09f27712f18 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 16 Dec 2024 09:18:37 +0100 Subject: [PATCH 49/54] Update ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp Co-authored-by: Felix Exner --- .../include/ur_controllers/freedrive_mode_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index ca66811d1..3dbf2c205 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: From b187719a5c5c0e87688cb927da945dc6450442dc Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 16 Dec 2024 09:18:54 +0100 Subject: [PATCH 50/54] Update ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp Co-authored-by: Felix Exner --- .../include/ur_controllers/freedrive_mode_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp index 3dbf2c205..043c9dca1 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -92,7 +92,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface std::shared_ptr> enable_freedrive_mode_sub_; - rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check on the topic + rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check for timeout on input mutable std::chrono::seconds timeout_interval_; void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); From 167526c474a0354f4c67f8196898738f137ec228 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 16 Dec 2024 09:19:20 +0100 Subject: [PATCH 51/54] Update ur_controllers/src/freedrive_mode_controller.cpp Co-authored-by: Felix Exner --- ur_controllers/src/freedrive_mode_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index f5ba423d3..ed289619f 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -1,5 +1,5 @@ -// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S +// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: From 5e5e0ef594ca76289923aa96b01bc5a2e2b82a42 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Mon, 16 Dec 2024 09:19:46 +0100 Subject: [PATCH 52/54] Update ur_controllers/src/freedrive_mode_controller.cpp Co-authored-by: Felix Exner --- ur_controllers/src/freedrive_mode_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index ed289619f..27b2afb51 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -261,7 +261,7 @@ void FreedriveModeController::start_timer() void FreedriveModeController::timeout_callback() { if (timer_started_ && freedrive_active_) { - RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since client is not reachable."); + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since no new message received."); freedrive_active_ = false; change_requested_ = true; From b1106f0ad7f29f2e06ee4eada8352b2c8dda39d5 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 17 Dec 2024 09:40:26 +0000 Subject: [PATCH 53/54] Added on_cleanup to guarantee RT safety and fixed flag init --- .../ur_controllers/freedrive_mode_controller.hpp | 2 ++ ur_controllers/src/freedrive_mode_controller.cpp | 15 +++++++++++---- 2 files changed, 13 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 043c9dca1..c32745954 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -80,6 +80,8 @@ class FreedriveModeController : public controller_interface::ControllerInterface CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; CallbackReturn on_init() override; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 27b2afb51..a3074e24a 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -117,7 +117,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta async_state_ = std::numeric_limits::quiet_NaN(); first_log_ = false; - logging_thread_running_ = false; + logging_thread_running_ = true; logging_requested_ = false; { @@ -164,16 +164,23 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta } controller_interface::CallbackReturn -ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) +ur_controllers::FreedriveModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) { abort_command_interface_->get().set_value(1.0); + + stop_logging_thread(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) +{ freedrive_active_ = false; freedrive_sub_timer_.reset(); timer_started_ = false; - stop_logging_thread(); - return CallbackReturn::SUCCESS; } From d4c0ed5b10dcd0bfec5cfe558c906bfa809cbfa6 Mon Sep 17 00:00:00 2001 From: Vincenzo Di Pentima Date: Tue, 17 Dec 2024 16:53:57 +0000 Subject: [PATCH 54/54] Updated callback name and added comment for Command Intefaces --- .../include/ur_controllers/freedrive_mode_controller.hpp | 4 ++-- ur_controllers/src/freedrive_mode_controller.cpp | 4 ++-- 2 files changed, 4 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 c32745954..04e90805e 100644 --- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -87,7 +87,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface CallbackReturn on_init() override; private: - // Command interfaces + // Command interfaces: optional is used only to avoid adding reference initialization std::optional> async_success_command_interface_; std::optional> enable_command_interface_; std::optional> abort_command_interface_; @@ -96,7 +96,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check for timeout on input mutable std::chrono::seconds timeout_interval_; - void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg); + void freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg); std::shared_ptr freedrive_param_listener_; freedrive_mode_controller::Params freedrive_params_; diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index a3074e24a..e1ae016e5 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -87,7 +87,7 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St // Subscriber definition enable_freedrive_mode_sub_ = get_node()->create_subscription( "~/enable_freedrive_mode", 10, - std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1)); + std::bind(&FreedriveModeController::freedrive_cmd_callback, this, std::placeholders::_1)); timer_started_ = false; @@ -230,7 +230,7 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat return controller_interface::return_type::OK; } -void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg) +void FreedriveModeController::freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg) { // Process the freedrive_mode command. if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {