From 8bc95d773f2476eeb18370a128b9f0c02ee98fd3 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 26 Feb 2024 11:11:07 +0100
Subject: [PATCH] Move communication setup to on_configure instead of
 on_activate (#936)

* 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<hardware_interface::CommandInterface> 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<urcl::UrDriver> ur_driver_;
   std::shared_ptr<std::thread> 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<hardware_interface::CommandInterface> 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();