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>
@@ -209,28 +210,32 @@ TEST_F(
209210 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
210211 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 );
211212
212- // When TestActuatorHardware is INACTIVE expect OK
213- EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
213+ // When TestActuatorHardware is INACTIVE expect not OK
214+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
214215 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
215- EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.001 , 1e-7 );
216- EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
216+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.0 , 1e-7 )
217+ << " Start interfaces with inactive should result in no change" ;
218+ EXPECT_FALSE (rm_->perform_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
217219 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
218- EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.101 , 1e-7 );
220+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.0 , 1e-7 )
221+ << " Start interfaces with inactive should result in no change" ;
219222
220- EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_actuator, empty_keys));
223+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_actuator, empty_keys));
221224 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
222- EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.102 , 1e-7 );
223- EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_actuator, empty_keys));
225+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.0 , 1e-7 )
226+ << " Start interfaces with inactive should result in no change" ;
227+ EXPECT_FALSE (rm_->perform_command_mode_switch (legal_keys_actuator, empty_keys));
224228 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
225- EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.202 , 1e-7 );
229+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.0 , 1e-7 )
230+ << " Start interfaces with inactive should result in no change" ;
226231
227232 EXPECT_TRUE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_actuator));
228233 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
229- EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.203 , 1e-7 );
234+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.001 , 1e-7 );
230235 EXPECT_TRUE (rm_->perform_command_mode_switch (empty_keys, legal_keys_actuator))
231- << " Inactive is OK" ;
236+ << " Inactive with empty start interfaces is OK" ;
232237 EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
233- EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.303 , 1e-7 );
238+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.101 , 1e-7 );
234239};
235240
236241// System : INACTIVE
@@ -242,54 +247,58 @@ TEST_F(
242247 lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE , " inactive" ,
243248 lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , " active" );
244249
245- // When TestSystemCommandModes is INACTIVE expect OK
246- EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_system, legal_keys_system));
247- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 1.0 );
250+ // When TestSystemCommandModes is INACTIVE expect not OK
251+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_system, legal_keys_system));
252+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 )
253+ << " Start interfaces with inactive should result in no change" ;
248254 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
249255 << " System interfaces shouldn't affect the actuator" ;
250- EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_system, legal_keys_system));
251- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101.0 );
256+ EXPECT_FALSE (rm_->perform_command_mode_switch (legal_keys_system, legal_keys_system));
257+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 )
258+ << " Start interfaces with inactive should result in no change" ;
252259 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
253260 << " System interfaces shouldn't affect the actuator" ;
254261
255- EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_system, empty_keys));
256- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 102.0 );
262+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_system, empty_keys));
263+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 )
264+ << " Start interfaces with inactive should result in no change" ;
257265 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
258266 << " System interfaces shouldn't affect the actuator" ;
259- EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_system, empty_keys));
260- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 202.0 );
267+ EXPECT_FALSE (rm_->perform_command_mode_switch (legal_keys_system, empty_keys));
268+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 )
269+ << " Start interfaces with inactive should result in no change" ;
261270 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
262271 << " System interfaces shouldn't affect the actuator" ;
263272
264273 EXPECT_TRUE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_system));
265- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 203 .0 );
274+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 1 .0 );
266275 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
267276 << " System interfaces shouldn't affect the actuator" ;
268277 EXPECT_FALSE (rm_->perform_command_mode_switch (empty_keys, legal_keys_system));
269- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
278+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
270279 EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
271280 << " System interfaces shouldn't affect the actuator" ;
272281
273282 // When TestActuatorHardware is ACTIVE expect OK
274283 EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
275- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
284+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
276285 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.001 , 1e-7 );
277286 EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
278- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
287+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
279288 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.101 , 1e-7 );
280289
281290 EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_actuator, empty_keys));
282- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
291+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
283292 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.102 , 1e-7 );
284293 EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_actuator, empty_keys));
285- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
294+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
286295 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.202 , 1e-7 );
287296
288297 EXPECT_TRUE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_actuator));
289- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
298+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
290299 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.203 , 1e-7 );
291300 EXPECT_TRUE (rm_->perform_command_mode_switch (empty_keys, legal_keys_actuator));
292- EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303 .0 );
301+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 101 .0 );
293302 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.303 , 1e-7 );
294303};
295304
@@ -351,6 +360,132 @@ TEST_F(
351360 EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.303 , 1e-7 );
352361};
353362
363+ // System : UNCONFIGURED
364+ // Actuator: ERROR
365+ TEST_F (
366+ ResourceManagerPreparePerformTest,
367+ when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing)
368+ {
369+ preconfigure_components (
370+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , " unconfigured" ,
371+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , " active" );
372+
373+ // When TestSystemCommandModes is UNCONFIGURED expect error
374+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_system, legal_keys_system));
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, legal_keys_system))
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 (legal_keys_system, empty_keys));
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 (legal_keys_system, empty_keys))
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+ EXPECT_FALSE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_system));
391+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
392+ EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 );
393+ EXPECT_FALSE (rm_->perform_command_mode_switch (empty_keys, legal_keys_system))
394+ << " The system HW component is unconfigured, so the perform should fail!" ;
395+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
396+ EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 );
397+
398+ // When TestActuatorHardware is ACTIVE expect OK
399+ EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
400+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
401+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.001 , 1e-7 );
402+ EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_actuator, legal_keys_actuator));
403+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
404+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.101 , 1e-7 );
405+
406+ EXPECT_TRUE (rm_->prepare_command_mode_switch (legal_keys_actuator, empty_keys));
407+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
408+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.102 , 1e-7 );
409+ EXPECT_TRUE (rm_->perform_command_mode_switch (legal_keys_actuator, empty_keys));
410+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
411+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.202 , 1e-7 );
412+
413+ EXPECT_TRUE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_actuator));
414+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
415+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.203 , 1e-7 );
416+ EXPECT_TRUE (rm_->perform_command_mode_switch (empty_keys, legal_keys_actuator));
417+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
418+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.303 , 1e-7 );
419+
420+ std::unique_ptr<hardware_interface::LoanedCommandInterface> claimed_actuator_velocity_command_ =
421+ std::make_unique<hardware_interface::LoanedCommandInterface>(
422+ rm_->claim_command_interface (" joint3/position" ));
423+
424+ auto status_map = rm_->get_components_status ();
425+ EXPECT_EQ (
426+ status_map[" TestSystemCommandModes" ].state .id (),
427+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
428+ EXPECT_EQ (
429+ status_map[" TestActuatorHardware" ].state .id (),
430+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
431+
432+ // Now deactivate with write deactivate value
433+ claimed_actuator_velocity_command_->set_value (test_constants::WRITE_DEACTIVATE_VALUE );
434+ rm_->write (node_.now (), rclcpp::Duration (0 , 1000000 ));
435+
436+ status_map = rm_->get_components_status ();
437+ EXPECT_EQ (
438+ status_map[" TestSystemCommandModes" ].state .id (),
439+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
440+ EXPECT_EQ (
441+ status_map[" TestActuatorHardware" ].state .id (),
442+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
443+
444+ // Similar to deactivate callback from write cycle
445+ EXPECT_TRUE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_actuator));
446+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
447+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.304 , 1e-7 );
448+ EXPECT_TRUE (rm_->perform_command_mode_switch (empty_keys, legal_keys_actuator));
449+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
450+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.404 , 1e-7 );
451+
452+ // Similar to the proximal activation
453+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_actuator, empty_keys));
454+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
455+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.404 , 1e-7 );
456+ EXPECT_FALSE (rm_->perform_command_mode_switch (legal_keys_actuator, empty_keys))
457+ << " Start interfaces with inactive should result in no change" ;
458+ EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 0.0 );
459+ EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.404 , 1e-7 )
460+ << " Start interfaces with inactive should result in no change" ;
461+
462+ // Now return ERROR with write fail value
463+ claimed_actuator_velocity_command_->set_value (test_constants::WRITE_FAIL_VALUE );
464+ rm_->write (node_.now (), rclcpp::Duration (0 , 1000000 ));
465+
466+ status_map = rm_->get_components_status ();
467+ EXPECT_EQ (
468+ status_map[" TestSystemCommandModes" ].state .id (),
469+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
470+ EXPECT_EQ (
471+ status_map[" TestActuatorHardware" ].state .id (),
472+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
473+
474+ EXPECT_FALSE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_actuator));
475+ 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));
478+ 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 );
480+
481+ EXPECT_FALSE (rm_->prepare_command_mode_switch (legal_keys_actuator, empty_keys));
482+ 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 );
484+ EXPECT_FALSE (rm_->perform_command_mode_switch (legal_keys_actuator, empty_keys));
485+ 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 );
487+ };
488+
354489// System : UNCONFIGURED
355490// Actuator: FINALIZED
356491TEST_F (
0 commit comments