Skip to content

Commit ed06e58

Browse files
saikishorsaikishor
committed
Add test case to test the deactivation and error on return values
1 parent 6983b1b commit ed06e58

1 file changed

Lines changed: 118 additions & 0 deletions

File tree

‎hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp‎

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
// Authors: Dr. Denis
1616

17+
#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
1718
#include "test_resource_manager.hpp"
1819

1920
#include <string>
@@ -351,6 +352,123 @@ TEST_F(
351352
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
352353
};
353354

355+
// System : UNCONFIGURED
356+
// Actuator: ERROR
357+
TEST_F(
358+
ResourceManagerPreparePerformTest,
359+
when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing)
360+
{
361+
preconfigure_components(
362+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured",
363+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");
364+
365+
// When TestSystemCommandModes is UNCONFIGURED expect error
366+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system));
367+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
368+
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
369+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system))
370+
<< "The system HW component is unconfigured, so the perform should fail!";
371+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
372+
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
373+
374+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys));
375+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
376+
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
377+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys))
378+
<< "The system HW component is unconfigured, so the perform should fail!";
379+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
380+
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
381+
382+
EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system));
383+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
384+
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
385+
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system))
386+
<< "The system HW component is unconfigured, so the perform should fail!";
387+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
388+
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
389+
390+
// When TestActuatorHardware is ACTIVE expect OK
391+
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
392+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
393+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
394+
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
395+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
396+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
397+
398+
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
399+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
400+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
401+
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
402+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
403+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);
404+
405+
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
406+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
407+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
408+
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
409+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
410+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
411+
412+
std::unique_ptr<hardware_interface::LoanedCommandInterface> claimed_actuator_velocity_command_ =
413+
std::make_unique<hardware_interface::LoanedCommandInterface>(
414+
rm_->claim_command_interface("joint3/position"));
415+
416+
auto status_map = rm_->get_components_status();
417+
EXPECT_EQ(
418+
status_map["TestSystemCommandModes"].state.id(),
419+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
420+
EXPECT_EQ(
421+
status_map["TestActuatorHardware"].state.id(),
422+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
423+
424+
// Now deactivate with write deactivate value
425+
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_DEACTIVATE_VALUE);
426+
rm_->write(node_.now(), rclcpp::Duration(0, 1000000));
427+
428+
status_map = rm_->get_components_status();
429+
EXPECT_EQ(
430+
status_map["TestSystemCommandModes"].state.id(),
431+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
432+
EXPECT_EQ(
433+
status_map["TestActuatorHardware"].state.id(),
434+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
435+
436+
// Similar to deactivate callback from write cycle
437+
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
438+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
439+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7);
440+
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
441+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
442+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
443+
444+
// Similar to the proximal activation
445+
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
446+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
447+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7);
448+
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
449+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
450+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
451+
452+
// Now return ERROR with write fail value
453+
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_FAIL_VALUE);
454+
rm_->write(node_.now(), rclcpp::Duration(0, 1000000));
455+
456+
status_map = rm_->get_components_status();
457+
EXPECT_EQ(
458+
status_map["TestSystemCommandModes"].state.id(),
459+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
460+
EXPECT_EQ(
461+
status_map["TestActuatorHardware"].state.id(),
462+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
463+
464+
EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
465+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
466+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
467+
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
468+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
469+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
470+
};
471+
354472
// System : UNCONFIGURED
355473
// Actuator: FINALIZED
356474
TEST_F(

0 commit comments

Comments
 (0)