Add states for a better state transition

ros-controls · Jun 13, 2024 · 1ddcda5 · 1ddcda5
1 parent 7854c7c
commit 1ddcda5
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 0 deletions.
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -191,6 +191,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  if (info_.is_async)
  {
  bool trigger_status = true;
+ if (next_trigger_ == TriggerType::WRITE)
+ {
+ RCLCPP_WARN(
+ rclcpp::get_logger("SystemInterface"),
+ "Trigger read called while write async handler call is still pending for hardware "
+ "interface : '%s'. Skipping read cycle and will wait for a write cycle!",
+ info_.name.c_str());
+ return return_type::OK;
+ }
  if (write_async_handler_->is_trigger_cycle_in_progress())
  {
  RCLCPP_WARN(
@@ -210,6 +219,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  info_.name.c_str());
  return return_type::OK;
  }
+ next_trigger_ = TriggerType::WRITE;
  }
  else
  {
@@ -246,6 +256,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  if (info_.is_async)
  {
  bool trigger_status = true;
+ if (next_trigger_ == TriggerType::READ)
+ {
+ RCLCPP_WARN(
+ rclcpp::get_logger("ActuatorInterface"),
+ "Trigger write called while read async handler call is still pending for hardware "
+ "interface : '%s'. Skipping write cycle and will wait for a read cycle!",
+ info_.name.c_str());
+ return return_type::OK;
+ }
  if (read_async_handler_->is_trigger_cycle_in_progress())
  {
  RCLCPP_WARN(
@@ -265,6 +284,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  info_.name.c_str());
  return return_type::OK;
  }
+ next_trigger_ = TriggerType::READ;
  }
  else
  {
@@ -313,6 +333,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  rclcpp_lifecycle::State lifecycle_state_;
  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> write_async_handler_;
+
+private:
+ enum class TriggerType
+ {
+ READ,
+ WRITE
+ } next_trigger_ = TriggerType::READ;
 };
 
 } // namespace hardware_interface

diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -181,6 +181,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  if (info_.is_async)
  {
  bool trigger_status = true;
+ if (next_trigger_ == TriggerType::WRITE)
+ {
+ RCLCPP_WARN(
+ rclcpp::get_logger("SystemInterface"),
+ "Trigger read called while write async handler call is still pending for hardware "
+ "interface : '%s'. Skipping read cycle and will wait for a write cycle!",
+ info_.name.c_str());
+ return return_type::OK;
+ }
  if (write_async_handler_->is_trigger_cycle_in_progress())
  {
  RCLCPP_WARN(
@@ -200,6 +209,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  info_.name.c_str());
  return return_type::OK;
  }
+ next_trigger_ = TriggerType::WRITE;
  }
  else
  {
@@ -236,6 +246,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  if (info_.is_async)
  {
  bool trigger_status = true;
+ if (next_trigger_ == TriggerType::READ)
+ {
+ RCLCPP_WARN(
+ rclcpp::get_logger("SystemInterface"),
+ "Trigger write called while read async handler call is still pending for hardware "
+ "interface : '%s'. Skipping write cycle and will wait for a read cycle!",
+ info_.name.c_str());
+ return return_type::OK;
+ }
  if (read_async_handler_->is_trigger_cycle_in_progress())
  {
  RCLCPP_WARN(
@@ -255,6 +274,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  info_.name.c_str());
  return return_type::OK;
  }
+ next_trigger_ = TriggerType::READ;
  }
  else
  {
@@ -303,6 +323,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  rclcpp_lifecycle::State lifecycle_state_;
  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> write_async_handler_;
+
+private:
+ enum class TriggerType
+ {
+ READ,
+ WRITE
+ } next_trigger_ = TriggerType::READ;
 };
 
 } // namespace hardware_interface