@@ -111,21 +111,6 @@ class SceneModuleInterface
111
111
112
112
virtual void acceptVisitor (const std::shared_ptr<SceneModuleVisitor> & visitor) const = 0;
113
113
114
- /* *
115
- * @brief Set the current_state_ based on updateState output.
116
- */
117
- virtual void updateCurrentState ()
118
- {
119
- const auto print = [this ](const auto & from, const auto & to) {
120
- RCLCPP_DEBUG (
121
- getLogger (), " [%s] Transit from %s to %s." , name_.c_str (), from.data (), to.data ());
122
- };
123
-
124
- const auto & from = current_state_;
125
- current_state_ = updateState ();
126
- print (magic_enum::enum_name (from), magic_enum::enum_name (current_state_));
127
- }
128
-
129
114
/* *
130
115
* @brief Return true if the module has request for execution (not necessarily feasible)
131
116
*/
@@ -159,6 +144,21 @@ class SceneModuleInterface
159
144
return isWaitingApproval () ? planWaitingApproval () : plan ();
160
145
}
161
146
147
+ /* *
148
+ * @brief Set the current_state_ based on updateState output.
149
+ */
150
+ void updateCurrentState ()
151
+ {
152
+ const auto print = [this ](const auto & from, const auto & to) {
153
+ RCLCPP_DEBUG (
154
+ getLogger (), " [%s] Transit from %s to %s." , name_.c_str (), from.data (), to.data ());
155
+ };
156
+
157
+ const auto & from = current_state_;
158
+ current_state_ = updateState ();
159
+ print (magic_enum::enum_name (from), magic_enum::enum_name (current_state_));
160
+ }
161
+
162
162
/* *
163
163
* @brief Called on the first time when the module goes into RUNNING.
164
164
*/
@@ -362,8 +362,84 @@ class SceneModuleInterface
362
362
return existApprovedRequest ();
363
363
}
364
364
365
+ /* *
366
+ * @brief Return SUCCESS if plan is not needed or plan is successfully finished,
367
+ * FAILURE if plan has failed, RUNNING if plan is on going.
368
+ * These condition is to be implemented in each modules.
369
+ */
370
+ ModuleStatus updateState ()
371
+ {
372
+ auto log_debug_throttled = [&](std::string_view message) -> void {
373
+ RCLCPP_DEBUG (getLogger (), " %s" , message.data ());
374
+ };
375
+ if (current_state_ == ModuleStatus::IDLE) {
376
+ if (canTransitIdleToRunningState ()) {
377
+ log_debug_throttled (" transiting from IDLE to RUNNING" );
378
+ return ModuleStatus::RUNNING;
379
+ }
380
+
381
+ log_debug_throttled (" transiting from IDLE to IDLE" );
382
+ return ModuleStatus::IDLE;
383
+ }
384
+
385
+ if (current_state_ == ModuleStatus::RUNNING) {
386
+ if (canTransitSuccessState ()) {
387
+ log_debug_throttled (" transiting from RUNNING to SUCCESS" );
388
+ return ModuleStatus::SUCCESS;
389
+ }
390
+
391
+ if (canTransitFailureState ()) {
392
+ log_debug_throttled (" transiting from RUNNING to FAILURE" );
393
+ return ModuleStatus::FAILURE;
394
+ }
395
+
396
+ if (canTransitWaitingApprovalState ()) {
397
+ log_debug_throttled (" transiting from RUNNING to WAITING_APPROVAL" );
398
+ return ModuleStatus::WAITING_APPROVAL;
399
+ }
400
+
401
+ log_debug_throttled (" transiting from RUNNING to RUNNING" );
402
+ return ModuleStatus::RUNNING;
403
+ }
404
+
405
+ if (current_state_ == ModuleStatus::WAITING_APPROVAL) {
406
+ if (canTransitSuccessState ()) {
407
+ log_debug_throttled (" transiting from WAITING_APPROVAL to SUCCESS" );
408
+ return ModuleStatus::SUCCESS;
409
+ }
410
+
411
+ if (canTransitFailureState ()) {
412
+ log_debug_throttled (" transiting from WAITING_APPROVAL to FAILURE" );
413
+ return ModuleStatus::FAILURE;
414
+ }
415
+
416
+ if (canTransitWaitingApprovalToRunningState ()) {
417
+ log_debug_throttled (" transiting from WAITING_APPROVAL to RUNNING" );
418
+ return ModuleStatus::RUNNING;
419
+ }
420
+
421
+ log_debug_throttled (" transiting from WAITING_APPROVAL to WAITING APPROVAL" );
422
+ return ModuleStatus::WAITING_APPROVAL;
423
+ }
424
+
425
+ if (current_state_ == ModuleStatus::SUCCESS) {
426
+ log_debug_throttled (" already SUCCESS" );
427
+ return ModuleStatus::SUCCESS;
428
+ }
429
+
430
+ if (current_state_ == ModuleStatus::FAILURE) {
431
+ log_debug_throttled (" already FAILURE" );
432
+ return ModuleStatus::FAILURE;
433
+ }
434
+
435
+ log_debug_throttled (" already IDLE" );
436
+ return ModuleStatus::IDLE;
437
+ }
438
+
365
439
std::string name_;
366
440
441
+ ModuleStatus current_state_{ModuleStatus::IDLE};
442
+
367
443
BehaviorModuleOutput previous_module_output_;
368
444
369
445
StopReason stop_reason_;
@@ -442,80 +518,6 @@ class SceneModuleInterface
442
518
}
443
519
}
444
520
445
- /* *
446
- * @brief Return SUCCESS if plan is not needed or plan is successfully finished,
447
- * FAILURE if plan has failed, RUNNING if plan is on going.
448
- * These condition is to be implemented in each modules.
449
- */
450
- virtual ModuleStatus updateState ()
451
- {
452
- auto log_debug_throttled = [&](std::string_view message) -> void {
453
- RCLCPP_WARN (getLogger (), " %s" , message.data ());
454
- };
455
- if (current_state_ == ModuleStatus::IDLE) {
456
- if (canTransitIdleToRunningState ()) {
457
- log_debug_throttled (" transiting from IDLE to RUNNING" );
458
- return ModuleStatus::RUNNING;
459
- }
460
-
461
- log_debug_throttled (" transiting from IDLE to IDLE" );
462
- return ModuleStatus::IDLE;
463
- }
464
-
465
- if (current_state_ == ModuleStatus::RUNNING) {
466
- if (canTransitSuccessState ()) {
467
- log_debug_throttled (" transiting from RUNNING to SUCCESS" );
468
- return ModuleStatus::SUCCESS;
469
- }
470
-
471
- if (canTransitFailureState ()) {
472
- log_debug_throttled (" transiting from RUNNING to FAILURE" );
473
- return ModuleStatus::FAILURE;
474
- }
475
-
476
- if (canTransitWaitingApprovalState ()) {
477
- log_debug_throttled (" transiting from RUNNING to WAITING_APPROVAL" );
478
- return ModuleStatus::WAITING_APPROVAL;
479
- }
480
-
481
- log_debug_throttled (" transiting from RUNNING to RUNNING" );
482
- return ModuleStatus::RUNNING;
483
- }
484
-
485
- if (current_state_ == ModuleStatus::WAITING_APPROVAL) {
486
- if (canTransitSuccessState ()) {
487
- log_debug_throttled (" transiting from WAITING_APPROVAL to SUCCESS" );
488
- return ModuleStatus::SUCCESS;
489
- }
490
-
491
- if (canTransitFailureState ()) {
492
- log_debug_throttled (" transiting from WAITING_APPROVAL to FAILURE" );
493
- return ModuleStatus::FAILURE;
494
- }
495
-
496
- if (canTransitWaitingApprovalToRunningState ()) {
497
- log_debug_throttled (" transiting from WAITING_APPROVAL to RUNNING" );
498
- return ModuleStatus::RUNNING;
499
- }
500
-
501
- log_debug_throttled (" transiting from WAITING_APPROVAL to WAITING APPROVAL" );
502
- return ModuleStatus::WAITING_APPROVAL;
503
- }
504
-
505
- if (current_state_ == ModuleStatus::SUCCESS) {
506
- log_debug_throttled (" already SUCCESS" );
507
- return ModuleStatus::SUCCESS;
508
- }
509
-
510
- if (current_state_ == ModuleStatus::FAILURE) {
511
- log_debug_throttled (" already FAILURE" );
512
- return ModuleStatus::FAILURE;
513
- }
514
-
515
- log_debug_throttled (" already IDLE" );
516
- return ModuleStatus::IDLE;
517
- }
518
-
519
521
/* *
520
522
* @brief Return true if the activation command is received from the RTC interface.
521
523
* If no RTC interface is registered, return true.
@@ -610,8 +612,6 @@ class SceneModuleInterface
610
612
PlanResult path_candidate_;
611
613
PlanResult path_reference_;
612
614
613
- ModuleStatus current_state_{ModuleStatus::IDLE};
614
-
615
615
std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map_;
616
616
617
617
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
0 commit comments