diff --git a/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp b/hardware_interface/include/hardware_interface/lifecycle_helpers.hpp new file mode 100644 index 0000000000..1358da3058 --- /dev/null +++ b/hardware_interface/include/hardware_interface/lifecycle_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__LIFECYCLE_HELPERS_HPP_ +#define HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_ + +#include + +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__LIFECYCLE_HELPERS_HPP_ diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index d586b463af..13f2090d1f 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lifecycle_helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -246,9 +247,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_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } @@ -276,9 +275,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_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 index 2cffa649fd..83b4c9face 100644 --- 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/lifecycle_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,9 +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_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 index 8f455a7bd5..6e96814c24 100644 --- 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/lifecycle_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,9 +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_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; } @@ -272,9 +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_UNCONFIGURED || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { return return_type::OK; }