From 91b6d9fc5a0c561e488b3a473d36313ac7ef1376 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 18:53:37 +0100 Subject: [PATCH] Protect 'read' and 'write' calls accoring to lifecycle #1175 --- hardware_interface/src/actuator.cpp | 4 ++++ hardware_interface/src/sensor.cpp | 4 ++++ hardware_interface/src/system.cpp | 8 ++++++++ 3 files changed, 16 insertions(+) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 694e92355ef..c436d1a210c 100644 --- 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 index ade9b8781a1..e634edbf353 100644 --- 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 index f8703a47bc9..701438af589 100644 --- 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 (