Skip to content

Commit abb82f5

Browse files
saikishorsaikishor
authored
Merge branch 'master' into wip/CM/statistics
2 parents 9f059f3 + 030aeb5 commit abb82f5

File tree

4 files changed

+47
-32
lines changed

4 files changed

+47
-32
lines changed

‎hardware_interface/src/actuator.cpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -388,9 +388,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
388388
last_write_cycle_time_ = time;
389389
return return_type::OK;
390390
}
391-
if (
392-
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
393-
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
391+
if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
394392
{
395393
const auto trigger_result = impl_->trigger_write(time, period);
396394
if (trigger_result.result == return_type::ERROR)

‎hardware_interface/test/test_component_interfaces.cpp

Lines changed: 16 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -813,14 +813,13 @@ TEST(TestComponentInterfaces, dummy_actuator)
813813
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id());
814814
EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label());
815815

816-
// Read and Write are working because it is INACTIVE
816+
// Read should work but write should not update the state because it is INACTIVE
817817
for (auto step = 0u; step < 10; ++step)
818818
{
819819
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
820820

821-
EXPECT_EQ(
822-
step * velocity_value, state_interfaces[0]->get_optional().value()); // position value
823-
EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity
821+
EXPECT_EQ(0.0, state_interfaces[0]->get_optional().value()); // position value
822+
EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value()); // velocity
824823

825824
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
826825
}
@@ -835,9 +834,10 @@ TEST(TestComponentInterfaces, dummy_actuator)
835834
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
836835

837836
EXPECT_EQ(
838-
(10 + step) * velocity_value,
839-
state_interfaces[0]->get_optional().value()); // position value
840-
EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value()); // velocity
837+
step * velocity_value,
838+
state_interfaces[0]->get_optional().value()); // position value
839+
EXPECT_EQ(
840+
step ? velocity_value : 0.0, state_interfaces[1]->get_optional().value()); // velocity
841841

842842
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
843843
}
@@ -851,7 +851,7 @@ TEST(TestComponentInterfaces, dummy_actuator)
851851
{
852852
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
853853

854-
EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value
854+
EXPECT_EQ(10 * velocity_value, state_interfaces[0]->get_optional().value()); // position value
855855
EXPECT_EQ(0, state_interfaces[1]->get_optional().value()); // velocity
856856

857857
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
@@ -939,17 +939,13 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
939939
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id());
940940
EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label());
941941

942-
// Read and Write are working because it is INACTIVE
942+
// Read should work but write should not update the state because it is INACTIVE
943943
for (auto step = 0u; step < 10; ++step)
944944
{
945945
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
946946

947-
EXPECT_EQ(
948-
step * velocity_value,
949-
state_interfaces[si_joint1_pos]->get_optional().value()); // position value
950-
EXPECT_EQ(
951-
step ? velocity_value : 0,
952-
state_interfaces[si_joint1_vel]->get_optional().value()); // velocity
947+
EXPECT_EQ(0.0, state_interfaces[si_joint1_pos]->get_optional().value()); // position value
948+
EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity
953949

954950
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
955951
}
@@ -964,9 +960,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
964960
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
965961

966962
EXPECT_EQ(
967-
(10 + step) * velocity_value,
963+
step * velocity_value,
968964
state_interfaces[si_joint1_pos]->get_optional().value()); // position value
969-
EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity
965+
EXPECT_EQ(
966+
step ? velocity_value : 0.0,
967+
state_interfaces[si_joint1_vel]->get_optional().value()); // velocity
970968

971969
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
972970
}
@@ -981,7 +979,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
981979
ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
982980

983981
EXPECT_EQ(
984-
20 * velocity_value,
982+
10 * velocity_value,
985983
state_interfaces[si_joint1_pos]->get_optional().value()); // position value
986984
EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity
987985

‎hardware_interface_testing/test/test_resource_manager.cpp

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,14 +99,26 @@ TEST_F(ResourceManagerTest, initialization_with_urdf)
9999
TEST_F(ResourceManagerTest, post_initialization_with_urdf)
100100
{
101101
TestableResourceManager rm(node_);
102+
// TODO(saikishor) : remove after the cleanup of deprecated API
103+
#pragma GCC diagnostic push
104+
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
102105
ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf));
106+
#pragma GCC diagnostic pop
107+
hardware_interface::ResourceManagerParams rm_params;
108+
rm_params.robot_description = ros2_control_test_assets::minimal_robot_urdf;
109+
rm_params.update_rate = 100;
110+
ASSERT_NO_THROW(rm.load_and_initialize_components(rm_params));
103111
}
104112

105113
void test_load_and_initialized_components_failure(const std::string & urdf)
106114
{
107115
rclcpp::Node node = rclcpp::Node("TestableResourceManager");
108116
TestableResourceManager rm(node);
117+
// TODO(saikishor) : remove after the cleanup of deprecated API
118+
#pragma GCC diagnostic push
119+
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
109120
ASSERT_FALSE(rm.load_and_initialize_components(urdf));
121+
#pragma GCC diagnostic pop
110122

111123
ASSERT_FALSE(rm.are_components_initialized());
112124

@@ -269,7 +281,11 @@ TEST_F(ResourceManagerTest, can_load_and_initialize_components_later)
269281
{
270282
TestableResourceManager rm(node_);
271283
ASSERT_FALSE(rm.are_components_initialized());
284+
// TODO(saikishor) : remove after the cleanup of deprecated API
285+
#pragma GCC diagnostic push
286+
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
272287
rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf);
288+
#pragma GCC diagnostic pop
273289
ASSERT_TRUE(rm.are_components_initialized());
274290
}
275291

‎hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp

Lines changed: 14 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -361,10 +361,10 @@ TEST_F(
361361
};
362362

363363
// System : UNCONFIGURED
364-
// Actuator: ERROR
364+
// Actuator: DEACTIVATED then WRITE_IGNORED
365365
TEST_F(
366366
ResourceManagerPreparePerformTest,
367-
when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing)
367+
when_actuator_deactivated_then_write_error_is_ignored_and_remains_inactive)
368368
{
369369
preconfigure_components(
370370
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured",
@@ -430,7 +430,8 @@ TEST_F(
430430
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
431431

432432
// Now deactivate with write deactivate value
433-
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_DEACTIVATE_VALUE);
433+
EXPECT_TRUE(
434+
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_DEACTIVATE_VALUE));
434435
rm_->write(node_.now(), rclcpp::Duration(0, 1000000));
435436

436437
status_map = rm_->get_components_status();
@@ -460,30 +461,32 @@ TEST_F(
460461
<< "Start interfaces with inactive should result in no change";
461462

462463
// Now return ERROR with write fail value
463-
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_FAIL_VALUE);
464+
EXPECT_TRUE(claimed_actuator_velocity_command_->set_value(test_constants::WRITE_FAIL_VALUE));
464465
rm_->write(node_.now(), rclcpp::Duration(0, 1000000));
465466

466467
status_map = rm_->get_components_status();
467468
EXPECT_EQ(
468469
status_map["TestSystemCommandModes"].state.id(),
469470
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
471+
// The component is INACTIVE, write does nothing, so no error is triggered. State remains
472+
// INACTIVE.
470473
EXPECT_EQ(
471474
status_map["TestActuatorHardware"].state.id(),
472-
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
475+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
473476

474-
EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
477+
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
475478
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
476-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
477-
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
479+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7);
480+
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
478481
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
479-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
482+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
480483

481484
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
482485
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
483-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
486+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
484487
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
485488
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
486-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
489+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
487490
};
488491

489492
// System : UNCONFIGURED

0 commit comments

Comments
 (0)