@@ -68,7 +68,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
68
68
pub_gear_cmd_ =
69
69
create_publisher<autoware_auto_vehicle_msgs::msg::GearCommand>(" ~/output/gear" , rclcpp::QoS{1 });
70
70
pub_mrm_state_ =
71
- create_publisher<tier4_system_msgs ::msg::MrmState>(" ~/output/mrm/state" , rclcpp::QoS{1 });
71
+ create_publisher<autoware_adapi_v1_msgs ::msg::MrmState>(" ~/output/mrm/state" , rclcpp::QoS{1 });
72
72
73
73
// Clients
74
74
client_mrm_pull_over_group_ = create_callback_group (rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -95,8 +95,8 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
95
95
mrm_emergency_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
96
96
operation_mode_state_ = std::make_shared<const autoware_adapi_v1_msgs::msg::OperationModeState>();
97
97
mrm_state_.stamp = this ->now ();
98
- mrm_state_.state = tier4_system_msgs ::msg::MrmState::NORMAL;
99
- mrm_state_.behavior . type = tier4_system_msgs ::msg::MrmBehavior ::NONE;
98
+ mrm_state_.state = autoware_adapi_v1_msgs ::msg::MrmState::NORMAL;
99
+ mrm_state_.behavior = autoware_adapi_v1_msgs ::msg::MrmState ::NONE;
100
100
is_operation_mode_availability_timeout = false ;
101
101
102
102
// Timer
@@ -227,31 +227,30 @@ void MrmHandler::publishMrmState()
227
227
228
228
void MrmHandler::operateMrm ()
229
229
{
230
- using tier4_system_msgs::msg::MrmBehavior;
231
- using tier4_system_msgs::msg::MrmState;
230
+ using autoware_adapi_v1_msgs::msg::MrmState;
232
231
233
232
if (mrm_state_.state == MrmState::NORMAL) {
234
233
// Cancel MRM behavior when returning to NORMAL state
235
- const auto current_mrm_behavior = MrmBehavior ::NONE;
236
- if (current_mrm_behavior == mrm_state_.behavior . type ) {
234
+ const auto current_mrm_behavior = MrmState ::NONE;
235
+ if (current_mrm_behavior == mrm_state_.behavior ) {
237
236
return ;
238
237
}
239
- if (requestMrmBehavior (mrm_state_.behavior . type , RequestType::CANCEL)) {
240
- mrm_state_.behavior . type = current_mrm_behavior;
238
+ if (requestMrmBehavior (mrm_state_.behavior , RequestType::CANCEL)) {
239
+ mrm_state_.behavior = current_mrm_behavior;
241
240
} else {
242
241
handleFailedRequest ();
243
242
}
244
243
return ;
245
244
}
246
245
if (mrm_state_.state == MrmState::MRM_OPERATING) {
247
246
const auto current_mrm_behavior = getCurrentMrmBehavior ();
248
- if (current_mrm_behavior == mrm_state_.behavior . type ) {
247
+ if (current_mrm_behavior == mrm_state_.behavior ) {
249
248
return ;
250
249
}
251
- if (!requestMrmBehavior (mrm_state_.behavior . type , RequestType::CANCEL)) {
250
+ if (!requestMrmBehavior (mrm_state_.behavior , RequestType::CANCEL)) {
252
251
handleFailedRequest ();
253
252
} else if (requestMrmBehavior (current_mrm_behavior, RequestType::CALL)) {
254
- mrm_state_.behavior . type = current_mrm_behavior;
253
+ mrm_state_.behavior = current_mrm_behavior;
255
254
} else {
256
255
handleFailedRequest ();
257
256
}
@@ -271,23 +270,21 @@ void MrmHandler::operateMrm()
271
270
272
271
void MrmHandler::handleFailedRequest ()
273
272
{
274
- using tier4_system_msgs::msg::MrmBehavior;
275
- using tier4_system_msgs::msg::MrmState;
273
+ using autoware_adapi_v1_msgs::msg::MrmState;
276
274
277
- if (requestMrmBehavior (MrmBehavior ::EMERGENCY_STOP, CALL)) {
275
+ if (requestMrmBehavior (MrmState ::EMERGENCY_STOP, CALL)) {
278
276
if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo (MrmState::MRM_OPERATING);
279
277
} else {
280
278
transitionTo (MrmState::MRM_FAILED);
281
279
}
282
- mrm_state_.behavior . type = MrmBehavior ::EMERGENCY_STOP;
280
+ mrm_state_.behavior = MrmState ::EMERGENCY_STOP;
283
281
}
284
282
285
283
bool MrmHandler::requestMrmBehavior (
286
- const tier4_system_msgs ::msg::MrmBehavior::_type_type & mrm_behavior,
284
+ const autoware_adapi_v1_msgs ::msg::MrmState::_behavior_type & mrm_behavior,
287
285
RequestType request_type) const
288
286
{
289
- using tier4_system_msgs::msg::MrmBehavior;
290
- using tier4_system_msgs::msg::MrmState;
287
+ using autoware_adapi_v1_msgs::msg::MrmState;
291
288
292
289
auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
293
290
if (request_type == RequestType::CALL) {
@@ -303,35 +300,35 @@ bool MrmHandler::requestMrmBehavior(
303
300
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;
304
301
305
302
const auto behavior2string = [](const int behavior) {
306
- if (behavior == MrmBehavior ::NONE) {
303
+ if (behavior == MrmState ::NONE) {
307
304
return " NONE" ;
308
305
}
309
- if (behavior == MrmBehavior ::PULL_OVER) {
306
+ if (behavior == MrmState ::PULL_OVER) {
310
307
return " PULL_OVER" ;
311
308
}
312
- if (behavior == MrmBehavior ::COMFORTABLE_STOP) {
309
+ if (behavior == MrmState ::COMFORTABLE_STOP) {
313
310
return " COMFORTABLE_STOP" ;
314
311
}
315
- if (behavior == MrmBehavior ::EMERGENCY_STOP) {
312
+ if (behavior == MrmState ::EMERGENCY_STOP) {
316
313
return " EMERGENCY_STOP" ;
317
314
}
318
315
const auto msg = " invalid behavior: " + std::to_string (behavior);
319
316
throw std::runtime_error (msg);
320
317
};
321
318
322
319
switch (mrm_behavior) {
323
- case MrmBehavior ::NONE:
320
+ case MrmState ::NONE:
324
321
RCLCPP_WARN (this ->get_logger (), " MRM behavior is None. Do nothing." );
325
322
return true ;
326
- case MrmBehavior ::PULL_OVER: {
323
+ case MrmState ::PULL_OVER: {
327
324
future = client_mrm_pull_over_->async_send_request (request).future .share ();
328
325
break ;
329
326
}
330
- case MrmBehavior ::COMFORTABLE_STOP: {
327
+ case MrmState ::COMFORTABLE_STOP: {
331
328
future = client_mrm_comfortable_stop_->async_send_request (request).future .share ();
332
329
break ;
333
330
}
334
- case MrmBehavior ::EMERGENCY_STOP: {
331
+ case MrmState ::EMERGENCY_STOP: {
335
332
future = client_mrm_emergency_stop_->async_send_request (request).future .share ();
336
333
break ;
337
334
}
@@ -436,7 +433,7 @@ void MrmHandler::onTimer()
436
433
437
434
void MrmHandler::transitionTo (const int new_state)
438
435
{
439
- using tier4_system_msgs ::msg::MrmState;
436
+ using autoware_adapi_v1_msgs ::msg::MrmState;
440
437
441
438
const auto state2string = [](const int state) {
442
439
if (state == MrmState::NORMAL) {
@@ -465,9 +462,8 @@ void MrmHandler::transitionTo(const int new_state)
465
462
466
463
void MrmHandler::updateMrmState ()
467
464
{
465
+ using autoware_adapi_v1_msgs::msg::MrmState;
468
466
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
469
- using tier4_system_msgs::msg::MrmBehavior;
470
- using tier4_system_msgs::msg::MrmState;
471
467
472
468
// Check emergency
473
469
const bool is_emergency = isEmergency ();
@@ -495,7 +491,7 @@ void MrmHandler::updateMrmState()
495
491
496
492
if (mrm_state_.state == MrmState::MRM_OPERATING) {
497
493
// TODO(TetsuKawa): Check MRC is accomplished
498
- if (mrm_state_.behavior . type == MrmBehavior ::PULL_OVER) {
494
+ if (mrm_state_.behavior == MrmState ::PULL_OVER) {
499
495
if (isStopped () && isArrivedAtGoal ()) {
500
496
transitionTo (MrmState::MRM_SUCCEEDED);
501
497
return ;
@@ -508,7 +504,7 @@ void MrmHandler::updateMrmState()
508
504
}
509
505
} else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) {
510
506
const auto current_mrm_behavior = getCurrentMrmBehavior ();
511
- if (current_mrm_behavior != mrm_state_.behavior . type ) {
507
+ if (current_mrm_behavior != mrm_state_.behavior ) {
512
508
transitionTo (MrmState::MRM_OPERATING);
513
509
}
514
510
} else if (mrm_state_.state == MrmState::MRM_FAILED) {
@@ -520,86 +516,85 @@ void MrmHandler::updateMrmState()
520
516
}
521
517
}
522
518
523
- tier4_system_msgs ::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior ()
519
+ autoware_adapi_v1_msgs ::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmBehavior ()
524
520
{
525
- using tier4_system_msgs::msg::MrmBehavior;
526
- using tier4_system_msgs::msg::MrmState;
521
+ using autoware_adapi_v1_msgs::msg::MrmState;
527
522
using tier4_system_msgs::msg::OperationModeAvailability;
528
523
529
524
// State machine
530
- if (mrm_state_.behavior . type == MrmBehavior ::NONE) {
525
+ if (mrm_state_.behavior == MrmState ::NONE) {
531
526
if (is_operation_mode_availability_timeout) {
532
- return MrmBehavior ::EMERGENCY_STOP;
527
+ return MrmState ::EMERGENCY_STOP;
533
528
}
534
529
if (operation_mode_availability_->pull_over ) {
535
530
if (param_.use_pull_over ) {
536
- return MrmBehavior ::PULL_OVER;
531
+ return MrmState ::PULL_OVER;
537
532
}
538
533
}
539
534
if (operation_mode_availability_->comfortable_stop ) {
540
535
if (param_.use_comfortable_stop ) {
541
- return MrmBehavior ::COMFORTABLE_STOP;
536
+ return MrmState ::COMFORTABLE_STOP;
542
537
}
543
538
}
544
539
if (!operation_mode_availability_->emergency_stop ) {
545
540
RCLCPP_WARN (this ->get_logger (), " no mrm operation available: operate emergency_stop" );
546
541
}
547
- return MrmBehavior ::EMERGENCY_STOP;
542
+ return MrmState ::EMERGENCY_STOP;
548
543
}
549
- if (mrm_state_.behavior . type == MrmBehavior ::PULL_OVER) {
544
+ if (mrm_state_.behavior == MrmState ::PULL_OVER) {
550
545
if (is_operation_mode_availability_timeout) {
551
- return MrmBehavior ::EMERGENCY_STOP;
546
+ return MrmState ::EMERGENCY_STOP;
552
547
}
553
548
if (operation_mode_availability_->pull_over ) {
554
549
if (param_.use_pull_over ) {
555
- return MrmBehavior ::PULL_OVER;
550
+ return MrmState ::PULL_OVER;
556
551
}
557
552
}
558
553
if (operation_mode_availability_->comfortable_stop ) {
559
554
if (param_.use_comfortable_stop ) {
560
- return MrmBehavior ::COMFORTABLE_STOP;
555
+ return MrmState ::COMFORTABLE_STOP;
561
556
}
562
557
}
563
558
if (!operation_mode_availability_->emergency_stop ) {
564
559
RCLCPP_WARN (this ->get_logger (), " no mrm operation available: operate emergency_stop" );
565
560
}
566
- return MrmBehavior ::EMERGENCY_STOP;
561
+ return MrmState ::EMERGENCY_STOP;
567
562
}
568
- if (mrm_state_.behavior . type == MrmBehavior ::COMFORTABLE_STOP) {
563
+ if (mrm_state_.behavior == MrmState ::COMFORTABLE_STOP) {
569
564
if (is_operation_mode_availability_timeout) {
570
- return MrmBehavior ::EMERGENCY_STOP;
565
+ return MrmState ::EMERGENCY_STOP;
571
566
}
572
567
if (isStopped () && operation_mode_availability_->pull_over ) {
573
568
if (param_.use_pull_over && param_.use_pull_over_after_stopped ) {
574
- return MrmBehavior ::PULL_OVER;
569
+ return MrmState ::PULL_OVER;
575
570
}
576
571
}
577
572
if (operation_mode_availability_->comfortable_stop ) {
578
573
if (param_.use_comfortable_stop ) {
579
- return MrmBehavior ::COMFORTABLE_STOP;
574
+ return MrmState ::COMFORTABLE_STOP;
580
575
}
581
576
}
582
577
if (!operation_mode_availability_->emergency_stop ) {
583
578
RCLCPP_WARN (this ->get_logger (), " no mrm operation available: operate emergency_stop" );
584
579
}
585
- return MrmBehavior ::EMERGENCY_STOP;
580
+ return MrmState ::EMERGENCY_STOP;
586
581
}
587
- if (mrm_state_.behavior . type == MrmBehavior ::EMERGENCY_STOP) {
582
+ if (mrm_state_.behavior == MrmState ::EMERGENCY_STOP) {
588
583
if (is_operation_mode_availability_timeout) {
589
- return MrmBehavior ::EMERGENCY_STOP;
584
+ return MrmState ::EMERGENCY_STOP;
590
585
}
591
586
if (isStopped () && operation_mode_availability_->pull_over ) {
592
587
if (param_.use_pull_over && param_.use_pull_over_after_stopped ) {
593
- return MrmBehavior ::PULL_OVER;
588
+ return MrmState ::PULL_OVER;
594
589
}
595
590
}
596
591
if (!operation_mode_availability_->emergency_stop ) {
597
592
RCLCPP_WARN (this ->get_logger (), " no mrm operation available: operate emergency_stop" );
598
593
}
599
- return MrmBehavior ::EMERGENCY_STOP;
594
+ return MrmState ::EMERGENCY_STOP;
600
595
}
601
596
602
- return mrm_state_.behavior . type ;
597
+ return mrm_state_.behavior ;
603
598
}
604
599
605
600
bool MrmHandler::isStopped ()
0 commit comments