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_
UniversalRobots · Feb 26, 2024 · da7b09f · da7b09f
1 parent 95dc22e
commit da7b09f
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 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
@@ -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
@@ -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();