From e0d0e456c788da8e051041ade99b1d914c77a8b8 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Mon, 26 Feb 2024 09:28:32 +0100 Subject: [PATCH] Move communication setup to on_configure instead of on_activate (#732) * Move communication setup to on_configure instead of on_activate * Cleanup in on_cleanup instead on_deactivate * Use std::atomic_bool for rtde_comm_has_been_started_ (cherry picked from commit da7b09f64b17ea3c4b26a8bdea600d218ea47f9a) --- .../ur_robot_driver/hardware_interface.hpp | 5 +++-- ur_robot_driver/src/hardware_interface.cpp | 21 +++++++++++++------ 2 files changed, 18 insertions(+), 8 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 63f762f09..fe1ede6f7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -96,8 +96,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector export_command_interfaces() final; + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) final; hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) final; - hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) final; + hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) final; hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) final; hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) final; @@ -223,7 +224,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::unique_ptr ur_driver_; std::shared_ptr async_thread_; - bool rtde_comm_has_been_started_ = false; + std::atomic_bool rtde_comm_has_been_started_ = false; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2434e5248..8ff61080e 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -59,7 +59,7 @@ URPositionHardwareInterface::~URPositionHardwareInterface() { // If the controller manager is shutdown via Ctrl + C the on_deactivate methods won't be called. // We therefore need to make sure to actually deactivate the communication - on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); } hardware_interface::CallbackReturn @@ -302,7 +302,7 @@ std::vector URPositionHardwareInterface::e } hardware_interface::CallbackReturn -URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) +URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Starting ...please wait..."); @@ -463,13 +463,22 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous } hardware_interface::CallbackReturn -URPositionHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& previous_state) +URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Activating HW interface"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn +URPositionHardwareInterface::on_cleanup(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping ...please wait..."); - async_thread_shutdown_ = true; - async_thread_->join(); - async_thread_.reset(); + if (async_thread_) { + async_thread_shutdown_ = true; + async_thread_->join(); + async_thread_.reset(); + } ur_driver_.reset();