@@ -52,10 +52,11 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
52
52
mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
53
53
is_hazard_status_timeout_ = false ;
54
54
55
- // Opertors
56
- emergency_stop_operator_ = std::make_unique<emergency_stop_operator::EmergencyStopOperator>(this );
57
- comfortable_stop_operator_ =
58
- std::make_unique<comfortable_stop_operator::ComfortableStopOperator>(this );
55
+ // MRM opertors
56
+ mrm_emergency_stop_operator_ =
57
+ std::make_unique<mrm_emergency_stop_operator::MrmEmergencyStopOperator>();
58
+ mrm_comfortable_stop_operator_ =
59
+ std::make_unique<mrm_comfortable_stop_operator::MrmComfortableStopOperator>();
59
60
60
61
// Timer
61
62
const auto update_period_ns = rclcpp::Rate (param_.update_rate ).period ();
@@ -131,15 +132,15 @@ void EmergencyHandler::callMrmBehavior(
131
132
return ;
132
133
}
133
134
if (mrm_behavior == MrmState::COMFORTABLE_STOP) {
134
- if (comfortable_stop_operator_ ->operate ()) {
135
+ if (mrm_comfortable_stop_operator_ ->operate ()) {
135
136
RCLCPP_WARN (this ->get_logger (), " Comfortable stop is operated" );
136
137
} else {
137
138
RCLCPP_ERROR (this ->get_logger (), " Comfortable stop is failed to operate" );
138
139
}
139
140
return ;
140
141
}
141
142
if (mrm_behavior == MrmState::EMERGENCY_STOP) {
142
- if (emergency_stop_operator_ ->operate ()) {
143
+ if (mrm_emergency_stop_operator_ ->operate ()) {
143
144
RCLCPP_WARN (this ->get_logger (), " Emergency stop is operated" );
144
145
} else {
145
146
RCLCPP_ERROR (this ->get_logger (), " Emergency stop is failed to operate" );
@@ -159,15 +160,15 @@ void EmergencyHandler::cancelMrmBehavior(
159
160
return ;
160
161
}
161
162
if (mrm_behavior == MrmState::COMFORTABLE_STOP) {
162
- if (comfortable_stop_operator_ ->cancel ()) {
163
+ if (mrm_comfortable_stop_operator_ ->cancel ()) {
163
164
RCLCPP_WARN (this ->get_logger (), " Comfortable stop is canceled" );
164
165
} else {
165
166
RCLCPP_ERROR (this ->get_logger (), " Comfortable stop is failed to cancel" );
166
167
}
167
168
return ;
168
169
}
169
170
if (mrm_behavior == MrmState::EMERGENCY_STOP) {
170
- if (emergency_stop_operator_ ->cancel ()) {
171
+ if (mrm_emergency_stop_operator_ ->cancel ()) {
171
172
RCLCPP_WARN (this ->get_logger (), " Emergency stop is canceled" );
172
173
} else {
173
174
RCLCPP_ERROR (this ->get_logger (), " Emergency stop is failed to cancel" );
0 commit comments