@@ -22,9 +22,11 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
22
22
{
23
23
// Parameter
24
24
param_.update_rate = declare_parameter<int >(" update_rate" , 10 );
25
- param_.timeout_operation_mode_availability = declare_parameter<double >(" timeout_operation_mode_availability" , 0.5 );
25
+ param_.timeout_operation_mode_availability =
26
+ declare_parameter<double >(" timeout_operation_mode_availability" , 0.5 );
26
27
param_.timeout_call_mrm_behavior = declare_parameter<double >(" timeout_call_mrm_behavior" , 0.01 );
27
- param_.timeout_cancel_mrm_behavior = declare_parameter<double >(" timeout_cancel_mrm_behavior" , 0.01 );
28
+ param_.timeout_cancel_mrm_behavior =
29
+ declare_parameter<double >(" timeout_cancel_mrm_behavior" , 0.01 );
28
30
param_.use_emergency_holding = declare_parameter<bool >(" use_emergency_holding" , false );
29
31
param_.timeout_emergency_recovery = declare_parameter<double >(" timeout_emergency_recovery" , 5.0 );
30
32
param_.timeout_takeover_request = declare_parameter<double >(" timeout_takeover_request" , 10.0 );
@@ -261,7 +263,7 @@ void MrmHandler::handlePostFailureRequest()
261
263
{
262
264
using autoware_adapi_v1_msgs::msg::MrmState;
263
265
264
- if (requestMrmBehavior (MrmState::EMERGENCY_STOP, CALL)) {
266
+ if (requestMrmBehavior (MrmState::EMERGENCY_STOP, CALL)) {
265
267
if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo (MrmState::MRM_OPERATING);
266
268
} else {
267
269
transitionTo (MrmState::MRM_FAILED);
@@ -270,13 +272,15 @@ void MrmHandler::handlePostFailureRequest()
270
272
}
271
273
272
274
bool MrmHandler::requestMrmBehavior (
273
- const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const
275
+ const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
276
+ bool call_or_cancel) const
274
277
{
275
278
using autoware_adapi_v1_msgs::msg::MrmState;
276
279
277
280
auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
278
281
request->operate = call_or_cancel; // true: call, false: cancel
279
- const auto duration = std::chrono::duration<double , std::ratio<1 >>(call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior );
282
+ const auto duration = std::chrono::duration<double , std::ratio<1 >>(
283
+ call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior );
280
284
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;
281
285
282
286
const auto behavior2string = [](const int behavior) {
@@ -317,17 +321,23 @@ bool MrmHandler::requestMrmBehavior(
317
321
return false ;
318
322
}
319
323
320
- if (future.wait_for (duration) == std::future_status::ready) {
324
+ if (future.wait_for (duration) == std::future_status::ready) {
321
325
const auto result = future.get ();
322
326
if (result->response .success == true ) {
323
- RCLCPP_WARN (this ->get_logger (), call_or_cancel ? " %s is operated." : " %s is canceled." , behavior2string (mrm_behavior));
327
+ RCLCPP_WARN (
328
+ this ->get_logger (), call_or_cancel ? " %s is operated." : " %s is canceled." ,
329
+ behavior2string (mrm_behavior));
324
330
return true ;
325
331
} else {
326
- RCLCPP_ERROR (this ->get_logger (), call_or_cancel ? " %s failed to operate." : " %s failed to cancel." , behavior2string (mrm_behavior));
332
+ RCLCPP_ERROR (
333
+ this ->get_logger (), call_or_cancel ? " %s failed to operate." : " %s failed to cancel." ,
334
+ behavior2string (mrm_behavior));
327
335
return false ;
328
336
}
329
337
} else {
330
- RCLCPP_ERROR (this ->get_logger (), call_or_cancel ? " %s call timed out." : " %s cancel timed out." , behavior2string (mrm_behavior));
338
+ RCLCPP_ERROR (
339
+ this ->get_logger (), call_or_cancel ? " %s call timed out." : " %s cancel timed out." ,
340
+ behavior2string (mrm_behavior));
331
341
return false ;
332
342
}
333
343
}
0 commit comments