Skip to content

Commit 07144ae

Browse files
committed
Revert "feat: change mrm state type to internal one (#1392)"
This reverts commit 835a843.
1 parent 835a843 commit 07144ae

File tree

2 files changed

+55
-61
lines changed

2 files changed

+55
-61
lines changed

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

+5-6
Original file line numberDiff line numberDiff line change
@@ -25,13 +25,12 @@
2525
// Autoware
2626
#include <tier4_autoware_utils/ros/update_param.hpp>
2727

28+
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
2829
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
2930
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
3031
#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
3132
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
32-
#include <tier4_system_msgs/msg/mrm_behavior.hpp>
3333
#include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
34-
#include <tier4_system_msgs/msg/mrm_state.hpp>
3534
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
3635
#include <tier4_system_msgs/srv/operate_mrm.hpp>
3736

@@ -118,9 +117,9 @@ class MrmHandler : public rclcpp::Node
118117
void publishHazardCmd();
119118
void publishGearCmd();
120119

121-
rclcpp::Publisher<tier4_system_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
120+
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
122121

123-
tier4_system_msgs::msg::MrmState mrm_state_;
122+
autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
124123
void publishMrmState();
125124

126125
// parameter callback
@@ -137,7 +136,7 @@ class MrmHandler : public rclcpp::Node
137136
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
138137

139138
bool requestMrmBehavior(
140-
const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior,
139+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
141140
RequestType request_type) const;
142141
void logMrmCallingResult(
143142
const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
@@ -165,7 +164,7 @@ class MrmHandler : public rclcpp::Node
165164
void updateMrmState();
166165
void operateMrm();
167166
void handleFailedRequest();
168-
tier4_system_msgs::msg::MrmBehavior::_type_type getCurrentMrmBehavior();
167+
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
169168
bool isStopped();
170169
bool isEmergency() const;
171170
bool isArrivedAtGoal();

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

+50-55
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
6868
pub_gear_cmd_ =
6969
create_publisher<autoware_auto_vehicle_msgs::msg::GearCommand>("~/output/gear", rclcpp::QoS{1});
7070
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});
7272

7373
// Clients
7474
client_mrm_pull_over_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -95,8 +95,8 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
9595
mrm_emergency_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
9696
operation_mode_state_ = std::make_shared<const autoware_adapi_v1_msgs::msg::OperationModeState>();
9797
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;
100100
is_operation_mode_availability_timeout = false;
101101

102102
// Timer
@@ -227,31 +227,30 @@ void MrmHandler::publishMrmState()
227227

228228
void MrmHandler::operateMrm()
229229
{
230-
using tier4_system_msgs::msg::MrmBehavior;
231-
using tier4_system_msgs::msg::MrmState;
230+
using autoware_adapi_v1_msgs::msg::MrmState;
232231

233232
if (mrm_state_.state == MrmState::NORMAL) {
234233
// 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) {
237236
return;
238237
}
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;
241240
} else {
242241
handleFailedRequest();
243242
}
244243
return;
245244
}
246245
if (mrm_state_.state == MrmState::MRM_OPERATING) {
247246
const auto current_mrm_behavior = getCurrentMrmBehavior();
248-
if (current_mrm_behavior == mrm_state_.behavior.type) {
247+
if (current_mrm_behavior == mrm_state_.behavior) {
249248
return;
250249
}
251-
if (!requestMrmBehavior(mrm_state_.behavior.type, RequestType::CANCEL)) {
250+
if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) {
252251
handleFailedRequest();
253252
} else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) {
254-
mrm_state_.behavior.type = current_mrm_behavior;
253+
mrm_state_.behavior = current_mrm_behavior;
255254
} else {
256255
handleFailedRequest();
257256
}
@@ -271,23 +270,21 @@ void MrmHandler::operateMrm()
271270

272271
void MrmHandler::handleFailedRequest()
273272
{
274-
using tier4_system_msgs::msg::MrmBehavior;
275-
using tier4_system_msgs::msg::MrmState;
273+
using autoware_adapi_v1_msgs::msg::MrmState;
276274

277-
if (requestMrmBehavior(MrmBehavior::EMERGENCY_STOP, CALL)) {
275+
if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) {
278276
if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING);
279277
} else {
280278
transitionTo(MrmState::MRM_FAILED);
281279
}
282-
mrm_state_.behavior.type = MrmBehavior::EMERGENCY_STOP;
280+
mrm_state_.behavior = MrmState::EMERGENCY_STOP;
283281
}
284282

285283
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,
287285
RequestType request_type) const
288286
{
289-
using tier4_system_msgs::msg::MrmBehavior;
290-
using tier4_system_msgs::msg::MrmState;
287+
using autoware_adapi_v1_msgs::msg::MrmState;
291288

292289
auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
293290
if (request_type == RequestType::CALL) {
@@ -303,35 +300,35 @@ bool MrmHandler::requestMrmBehavior(
303300
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;
304301

305302
const auto behavior2string = [](const int behavior) {
306-
if (behavior == MrmBehavior::NONE) {
303+
if (behavior == MrmState::NONE) {
307304
return "NONE";
308305
}
309-
if (behavior == MrmBehavior::PULL_OVER) {
306+
if (behavior == MrmState::PULL_OVER) {
310307
return "PULL_OVER";
311308
}
312-
if (behavior == MrmBehavior::COMFORTABLE_STOP) {
309+
if (behavior == MrmState::COMFORTABLE_STOP) {
313310
return "COMFORTABLE_STOP";
314311
}
315-
if (behavior == MrmBehavior::EMERGENCY_STOP) {
312+
if (behavior == MrmState::EMERGENCY_STOP) {
316313
return "EMERGENCY_STOP";
317314
}
318315
const auto msg = "invalid behavior: " + std::to_string(behavior);
319316
throw std::runtime_error(msg);
320317
};
321318

322319
switch (mrm_behavior) {
323-
case MrmBehavior::NONE:
320+
case MrmState::NONE:
324321
RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing.");
325322
return true;
326-
case MrmBehavior::PULL_OVER: {
323+
case MrmState::PULL_OVER: {
327324
future = client_mrm_pull_over_->async_send_request(request).future.share();
328325
break;
329326
}
330-
case MrmBehavior::COMFORTABLE_STOP: {
327+
case MrmState::COMFORTABLE_STOP: {
331328
future = client_mrm_comfortable_stop_->async_send_request(request).future.share();
332329
break;
333330
}
334-
case MrmBehavior::EMERGENCY_STOP: {
331+
case MrmState::EMERGENCY_STOP: {
335332
future = client_mrm_emergency_stop_->async_send_request(request).future.share();
336333
break;
337334
}
@@ -436,7 +433,7 @@ void MrmHandler::onTimer()
436433

437434
void MrmHandler::transitionTo(const int new_state)
438435
{
439-
using tier4_system_msgs::msg::MrmState;
436+
using autoware_adapi_v1_msgs::msg::MrmState;
440437

441438
const auto state2string = [](const int state) {
442439
if (state == MrmState::NORMAL) {
@@ -465,9 +462,8 @@ void MrmHandler::transitionTo(const int new_state)
465462

466463
void MrmHandler::updateMrmState()
467464
{
465+
using autoware_adapi_v1_msgs::msg::MrmState;
468466
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
469-
using tier4_system_msgs::msg::MrmBehavior;
470-
using tier4_system_msgs::msg::MrmState;
471467

472468
// Check emergency
473469
const bool is_emergency = isEmergency();
@@ -495,7 +491,7 @@ void MrmHandler::updateMrmState()
495491

496492
if (mrm_state_.state == MrmState::MRM_OPERATING) {
497493
// TODO(TetsuKawa): Check MRC is accomplished
498-
if (mrm_state_.behavior.type == MrmBehavior::PULL_OVER) {
494+
if (mrm_state_.behavior == MrmState::PULL_OVER) {
499495
if (isStopped() && isArrivedAtGoal()) {
500496
transitionTo(MrmState::MRM_SUCCEEDED);
501497
return;
@@ -508,7 +504,7 @@ void MrmHandler::updateMrmState()
508504
}
509505
} else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) {
510506
const auto current_mrm_behavior = getCurrentMrmBehavior();
511-
if (current_mrm_behavior != mrm_state_.behavior.type) {
507+
if (current_mrm_behavior != mrm_state_.behavior) {
512508
transitionTo(MrmState::MRM_OPERATING);
513509
}
514510
} else if (mrm_state_.state == MrmState::MRM_FAILED) {
@@ -520,86 +516,85 @@ void MrmHandler::updateMrmState()
520516
}
521517
}
522518

523-
tier4_system_msgs::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior()
519+
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmBehavior()
524520
{
525-
using tier4_system_msgs::msg::MrmBehavior;
526-
using tier4_system_msgs::msg::MrmState;
521+
using autoware_adapi_v1_msgs::msg::MrmState;
527522
using tier4_system_msgs::msg::OperationModeAvailability;
528523

529524
// State machine
530-
if (mrm_state_.behavior.type == MrmBehavior::NONE) {
525+
if (mrm_state_.behavior == MrmState::NONE) {
531526
if (is_operation_mode_availability_timeout) {
532-
return MrmBehavior::EMERGENCY_STOP;
527+
return MrmState::EMERGENCY_STOP;
533528
}
534529
if (operation_mode_availability_->pull_over) {
535530
if (param_.use_pull_over) {
536-
return MrmBehavior::PULL_OVER;
531+
return MrmState::PULL_OVER;
537532
}
538533
}
539534
if (operation_mode_availability_->comfortable_stop) {
540535
if (param_.use_comfortable_stop) {
541-
return MrmBehavior::COMFORTABLE_STOP;
536+
return MrmState::COMFORTABLE_STOP;
542537
}
543538
}
544539
if (!operation_mode_availability_->emergency_stop) {
545540
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
546541
}
547-
return MrmBehavior::EMERGENCY_STOP;
542+
return MrmState::EMERGENCY_STOP;
548543
}
549-
if (mrm_state_.behavior.type == MrmBehavior::PULL_OVER) {
544+
if (mrm_state_.behavior == MrmState::PULL_OVER) {
550545
if (is_operation_mode_availability_timeout) {
551-
return MrmBehavior::EMERGENCY_STOP;
546+
return MrmState::EMERGENCY_STOP;
552547
}
553548
if (operation_mode_availability_->pull_over) {
554549
if (param_.use_pull_over) {
555-
return MrmBehavior::PULL_OVER;
550+
return MrmState::PULL_OVER;
556551
}
557552
}
558553
if (operation_mode_availability_->comfortable_stop) {
559554
if (param_.use_comfortable_stop) {
560-
return MrmBehavior::COMFORTABLE_STOP;
555+
return MrmState::COMFORTABLE_STOP;
561556
}
562557
}
563558
if (!operation_mode_availability_->emergency_stop) {
564559
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
565560
}
566-
return MrmBehavior::EMERGENCY_STOP;
561+
return MrmState::EMERGENCY_STOP;
567562
}
568-
if (mrm_state_.behavior.type == MrmBehavior::COMFORTABLE_STOP) {
563+
if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) {
569564
if (is_operation_mode_availability_timeout) {
570-
return MrmBehavior::EMERGENCY_STOP;
565+
return MrmState::EMERGENCY_STOP;
571566
}
572567
if (isStopped() && operation_mode_availability_->pull_over) {
573568
if (param_.use_pull_over && param_.use_pull_over_after_stopped) {
574-
return MrmBehavior::PULL_OVER;
569+
return MrmState::PULL_OVER;
575570
}
576571
}
577572
if (operation_mode_availability_->comfortable_stop) {
578573
if (param_.use_comfortable_stop) {
579-
return MrmBehavior::COMFORTABLE_STOP;
574+
return MrmState::COMFORTABLE_STOP;
580575
}
581576
}
582577
if (!operation_mode_availability_->emergency_stop) {
583578
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
584579
}
585-
return MrmBehavior::EMERGENCY_STOP;
580+
return MrmState::EMERGENCY_STOP;
586581
}
587-
if (mrm_state_.behavior.type == MrmBehavior::EMERGENCY_STOP) {
582+
if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) {
588583
if (is_operation_mode_availability_timeout) {
589-
return MrmBehavior::EMERGENCY_STOP;
584+
return MrmState::EMERGENCY_STOP;
590585
}
591586
if (isStopped() && operation_mode_availability_->pull_over) {
592587
if (param_.use_pull_over && param_.use_pull_over_after_stopped) {
593-
return MrmBehavior::PULL_OVER;
588+
return MrmState::PULL_OVER;
594589
}
595590
}
596591
if (!operation_mode_availability_->emergency_stop) {
597592
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
598593
}
599-
return MrmBehavior::EMERGENCY_STOP;
594+
return MrmState::EMERGENCY_STOP;
600595
}
601596

602-
return mrm_state_.behavior.type;
597+
return mrm_state_.behavior;
603598
}
604599

605600
bool MrmHandler::isStopped()

0 commit comments

Comments
 (0)