add helper function to discard lifecycle state that doesn't need any …

…action
ros-controls · Aug 26, 2024 · fd1a50a · fd1a50a
1 parent 1e453bc
commit fd1a50a
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 20 deletions.
diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp
@@ -22,6 +22,7 @@
 #include "hardware_interface/actuator_interface.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/helpers.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/visibility_control.h"
 #include "rclcpp/duration.hpp"

diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp
@@ -0,0 +1,30 @@
+// Copyright 2024 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef HARDWARE_INTERFACE__HELPERS_HPP_
+#define HARDWARE_INTERFACE__HELPERS_HPP_
+
+#include <lifecycle_msgs/msg/state.hpp>
+
+namespace hardware_interface
+{
+constexpr bool lifecycleStateThatRequiresNoAction(const lifecycle_msgs::msg::State::_id_type state)
+{
+  return state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
+         state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+         state == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED;
+}
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__HELPERS_HPP_
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -246,10 +246,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p
       impl_->get_name().c_str());
     return return_type::OK;
   }
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     return return_type::OK;
   }
@@ -277,10 +274,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
       impl_->get_name().c_str());
     return return_type::OK;
   }
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     return return_type::OK;
   }

diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
@@ -21,6 +21,7 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/helpers.hpp"
 #include "hardware_interface/sensor_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -224,10 +225,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per
       impl_->get_name().c_str());
     return return_type::OK;
   }
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     return return_type::OK;
   }

diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -21,6 +21,7 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/helpers.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -242,10 +243,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per
       impl_->get_name().c_str());
     return return_type::OK;
   }
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     return return_type::OK;
   }
@@ -273,10 +271,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe
       impl_->get_name().c_str());
     return return_type::OK;
   }
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     return return_type::OK;
   }