14
14
15
15
// Authors: Dr. Denis
16
16
17
+ #include " ros2_control_test_assets/test_hardware_interface_constants.hpp"
17
18
#include " test_resource_manager.hpp"
18
19
19
20
#include < string>
@@ -209,28 +210,32 @@ TEST_F(
209
210
EXPECT_EQ (claimed_system_acceleration_state_->get_optional ().value (), 303.0 );
210
211
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 );
211
212
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));
214
215
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));
217
219
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" ;
219
222
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));
221
224
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));
224
228
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" ;
226
231
227
232
EXPECT_TRUE (rm_->prepare_command_mode_switch (empty_keys, legal_keys_actuator));
228
233
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 );
230
235
EXPECT_TRUE (rm_->perform_command_mode_switch (empty_keys, legal_keys_actuator))
231
- << " Inactive is OK" ;
236
+ << " Inactive with empty start interfaces is OK" ;
232
237
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 );
234
239
};
235
240
236
241
// System : INACTIVE
@@ -242,54 +247,58 @@ TEST_F(
242
247
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, " inactive" ,
243
248
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, " active" );
244
249
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" ;
248
254
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
249
255
<< " 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" ;
252
259
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
253
260
<< " System interfaces shouldn't affect the actuator" ;
254
261
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" ;
257
265
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
258
266
<< " 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" ;
261
270
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
262
271
<< " System interfaces shouldn't affect the actuator" ;
263
272
264
273
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 );
266
275
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
267
276
<< " System interfaces shouldn't affect the actuator" ;
268
277
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 );
270
279
EXPECT_EQ (claimed_actuator_position_state_->get_optional ().value (), 0.0 )
271
280
<< " System interfaces shouldn't affect the actuator" ;
272
281
273
282
// When TestActuatorHardware is ACTIVE expect OK
274
283
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 );
276
285
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.001 , 1e-7 );
277
286
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 );
279
288
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.101 , 1e-7 );
280
289
281
290
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 );
283
292
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.102 , 1e-7 );
284
293
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 );
286
295
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.202 , 1e-7 );
287
296
288
297
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 );
290
299
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.203 , 1e-7 );
291
300
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 );
293
302
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.303 , 1e-7 );
294
303
};
295
304
@@ -351,6 +360,132 @@ TEST_F(
351
360
EXPECT_NEAR (claimed_actuator_position_state_->get_optional ().value (), 0.303 , 1e-7 );
352
361
};
353
362
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
+
354
489
// System : UNCONFIGURED
355
490
// Actuator: FINALIZED
356
491
TEST_F (
0 commit comments