add prepare_for_activation for resetting the variables

ros-controls · Feb 2, 2025 · c9c16ab · c9c16ab
1 parent 4ac00a1
commit c9c16ab
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 0 deletions.
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -526,6 +526,18 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Prepare for the activation of the hardware.
+  /**
+   * This method is called before the hardware is activated by the resource manager.
+   */
+  void prepare_for_activation()
+  {
+    read_return_info_.store(return_type::OK, std::memory_order_release);
+    read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release);
+    write_return_info_.store(return_type::OK, std::memory_order_release);
+    write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release);
+  }
+
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription

diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -555,6 +555,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Prepare for the activation of the hardware.
+  /**
+   * This method is called before the hardware is activated by the resource manager.
+   */
+  void prepare_for_activation()
+  {
+    read_return_info_.store(return_type::OK, std::memory_order_release);
+    read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release);
+    write_return_info_.store(return_type::OK, std::memory_order_release);
+    write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release);
+  }
+
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription

diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -145,6 +145,7 @@ const rclcpp_lifecycle::State & Actuator::activate()
   write_statistics_.reset_statistics();
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->prepare_for_activation();
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:

diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -143,6 +143,7 @@ const rclcpp_lifecycle::State & System::activate()
   write_statistics_.reset_statistics();
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->prepare_for_activation();
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS: