Don't return error also on finalized state.

ros-controls · Jan 9, 2024 · ca60064 · ca60064
1 parent 5485f79
commit ca60064
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 5 deletions.
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -218,7 +218,8 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_
 
 return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+      impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
   {
     return return_type::OK;
   }
@@ -238,7 +239,8 @@ 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)
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+      impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
   {
     return return_type::OK;
   }

diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
@@ -195,7 +195,8 @@ 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)
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+      impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
   {
     return return_type::OK;
   }

diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -214,7 +214,8 @@ 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)
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+      impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
   {
     return return_type::OK;
   }
@@ -234,7 +235,8 @@ 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)
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+      impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
   {
     return return_type::OK;
   }