Protect 'read' and 'write' calls accoring to lifecycle ros-controls#1175

SchillingRobotics · Dec 18, 2023 · 91b6d9f · 91b6d9f
1 parent 8cc2f10
commit 91b6d9f
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 0 deletions.
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -235,6 +235,10 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p
 
 return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
   if (

diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
@@ -195,6 +195,10 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st
 
 return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
   if (

diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -214,6 +214,10 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st
 
 return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
   if (
@@ -231,6 +235,10 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per
 
 return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
   if (