Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix multiple calls of cleanup when system is unconfigured #1279

Merged
merged 12 commits into from
Jan 12, 2024
Merged

Fix multiple calls of cleanup when system is unconfigured #1279

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
63b725d
fix multiple system rise cleanup
anscipione Nov 20, 2023
5ad0199
Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…
anscipione Dec 12, 2023
653f2d0
Merge branch 'ros-controls:master' into fix_multiple_cleanup_on_uncon…
anscipione Dec 13, 2023
5184402
Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…
anscipione Dec 14, 2023
40a3d14
Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…
anscipione Dec 22, 2023
1c238f0
Apply suggestions from code review
anscipione Jan 7, 2024
7f39020
Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…
anscipione Jan 7, 2024
dfe0a6e
Make changes unified.
destogl Jan 8, 2024
b183b0f
Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…
bmagyar Jan 8, 2024
5485f79
Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…
destogl Jan 9, 2024
ca60064
Don't return error also on finalized state.
destogl Jan 9, 2024
2aa0d02
Fix formatting and tests.
destogl Jan 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 12 additions & 2 deletions hardware_interface/src/actuator.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading