@@ -226,7 +226,7 @@ void MrmHandler::operateMrm()
226
226
if (current_mrm_behavior == mrm_state_.behavior ) {
227
227
return ;
228
228
}
229
- if (requestMrmBehavior (mrm_state_.behavior , CANCEL)) {
229
+ if (requestMrmBehavior (mrm_state_.behavior , RequestType:: CANCEL)) {
230
230
mrm_state_.behavior = current_mrm_behavior;
231
231
} else {
232
232
handlePostFailureRequest ();
@@ -238,9 +238,9 @@ void MrmHandler::operateMrm()
238
238
if (current_mrm_behavior == mrm_state_.behavior ) {
239
239
return ;
240
240
}
241
- if (!requestMrmBehavior (mrm_state_.behavior , CANCEL)) {
241
+ if (!requestMrmBehavior (mrm_state_.behavior , RequestType:: CANCEL)) {
242
242
handlePostFailureRequest ();
243
- } else if (requestMrmBehavior (current_mrm_behavior, CALL)) {
243
+ } else if (requestMrmBehavior (current_mrm_behavior, RequestType:: CALL)) {
244
244
mrm_state_.behavior = current_mrm_behavior;
245
245
} else {
246
246
handlePostFailureRequest ();
@@ -272,15 +272,20 @@ void MrmHandler::handlePostFailureRequest()
272
272
}
273
273
274
274
bool MrmHandler::requestMrmBehavior (
275
- const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
276
- bool call_or_cancel) const
275
+ const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const
277
276
{
278
277
using autoware_adapi_v1_msgs::msg::MrmState;
279
278
280
279
auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
281
- request->operate = call_or_cancel; // true: call, false: cancel
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
+ if (request_type == RequestType::CALL) {
281
+ request->operate = true ;
282
+ } else if (request_type == RequestType::CANCEL) {
283
+ request->operate = false ;
284
+ } else {
285
+ RCLCPP_ERROR (this ->get_logger (), " invalid request type: %d" , request_type);
286
+ return false ;
287
+ }
288
+ const auto duration = std::chrono::duration<double , std::ratio<1 >>(request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior );
284
289
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;
285
290
286
291
const auto behavior2string = [](const int behavior) {
@@ -324,20 +329,14 @@ bool MrmHandler::requestMrmBehavior(
324
329
if (future.wait_for (duration) == std::future_status::ready) {
325
330
const auto result = future.get ();
326
331
if (result->response .success == true ) {
327
- RCLCPP_WARN (
328
- this ->get_logger (), call_or_cancel ? " %s is operated." : " %s is canceled." ,
329
- behavior2string (mrm_behavior));
332
+ RCLCPP_WARN (this ->get_logger (), request->operate ? " %s is operated." : " %s is canceled." , behavior2string (mrm_behavior));
330
333
return true ;
331
334
} else {
332
- RCLCPP_ERROR (
333
- this ->get_logger (), call_or_cancel ? " %s failed to operate." : " %s failed to cancel." ,
334
- behavior2string (mrm_behavior));
335
+ RCLCPP_ERROR (this ->get_logger (), request->operate ? " %s failed to operate." : " %s failed to cancel." , behavior2string (mrm_behavior));
335
336
return false ;
336
337
}
337
338
} else {
338
- RCLCPP_ERROR (
339
- this ->get_logger (), call_or_cancel ? " %s call timed out." : " %s cancel timed out." ,
340
- behavior2string (mrm_behavior));
339
+ RCLCPP_ERROR (this ->get_logger (), request->operate ? " %s call timed out." : " %s cancel timed out." , behavior2string (mrm_behavior));
341
340
return false ;
342
341
}
343
342
}
0 commit comments