diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 694e92355e..3df37fb5a0 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -218,7 +218,10 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -235,7 +238,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) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ade9b8781a..84a3aaf2f0 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -197,6 +197,11 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per { // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return return_type::OK; + } + if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f8703a47bc..fe965c5e55 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -216,6 +216,11 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per { // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return return_type::OK; + } + if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -233,6 +238,11 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe { // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return return_type::OK; + } + if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 128771058b..292c05e09e 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -446,17 +446,17 @@ TEST(TestComponentInterfaces, dummy_actuator) double velocity_value = 1.0; command_interfaces[0].set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } state = actuator_hw.configure(); @@ -587,12 +587,12 @@ TEST(TestComponentInterfaces, dummy_system) command_interfaces[0].set_value(velocity_value); // velocity command_interfaces[1].set_value(velocity_value); // velocity command_interfaces[2].set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity @@ -601,7 +601,7 @@ TEST(TestComponentInterfaces, dummy_system) ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } state = system_hw.configure();