Skip to content

Commit

Permalink
Fix return of ERROR and calls of cleanup when system is unconfigured …
Browse files Browse the repository at this point in the history 
…of finalized (#1279)

---------

Co-authored-by: Andrea Scipione <andrea.scipione@gmail.com>
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
  • Loading branch information
@destogl @anscipione @bmagyar
3 people authored Jan 12, 2024
1 parent 71506d5 commit acbeeea
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 15 deletions.
14 changes: 12 additions & 2 deletions hardware_interface/src/actuator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,7 +218,12 @@ 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 ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
{
return return_type::OK;
}
return_type result = return_type::ERROR;
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand All @@ -235,7 +240,12 @@ 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 ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
{
return return_type::OK;
}
return_type result = return_type::ERROR;
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand Down
7 changes: 6 additions & 1 deletion hardware_interface/src/sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,12 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st

return_type Sensor::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 ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
{
return return_type::OK;
}
return_type result = return_type::ERROR;
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand Down
14 changes: 12 additions & 2 deletions hardware_interface/src/system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,12 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st

return_type System::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 ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
{
return return_type::OK;
}
return_type result = return_type::ERROR;
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand All @@ -231,7 +236,12 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per

return_type System::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 ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
{
return return_type::OK;
}
return_type result = return_type::ERROR;
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand Down
32 changes: 22 additions & 10 deletions hardware_interface/test/test_component_interfaces.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -496,12 +496,12 @@ TEST(TestComponentInterfaces, dummy_actuator)
// Noting should change because it is FINALIZED
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));

EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value
EXPECT_EQ(0, 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));
}

EXPECT_EQ(
Expand Down Expand Up @@ -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
Expand All @@ -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();
Expand Down Expand Up @@ -649,7 +649,7 @@ TEST(TestComponentInterfaces, dummy_system)
// Noting should change because it is FINALIZED
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));

EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value
EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity
Expand All @@ -658,7 +658,7 @@ TEST(TestComponentInterfaces, dummy_system)
EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value
EXPECT_EQ(0.0, 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));
}

EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {}));
Expand Down Expand Up @@ -841,6 +841,12 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior)
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());

// Noting should change because it is UNCONFIGURED
for (auto step = 0u; step < 10; ++step)
{
ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD));
}

// activate again and expect reset values
state = sensor_hw.configure();
EXPECT_EQ(state_interfaces[0].get_value(), 0.0);
Expand All @@ -860,6 +866,12 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior)
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label());

// Noting should change because it is FINALIZED
for (auto step = 0u; step < 10; ++step)
{
ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD));
}

// can not change state anymore
state = sensor_hw.configure();
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id());
Expand Down

0 comments on commit acbeeea

Please sign in to comment.