From d421e38a1809d6210f08b97c4ebedb605c4c9479 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sat, 2 Mar 2024 01:41:11 +0900 Subject: [PATCH 1/7] feat: add timeouts of request services Signed-off-by: TetsuKawa --- .../mrm_handler/config/mrm_handler.param.yaml | 2 + system/mrm_handler/image/mrm-state.svg | 360 +----------------- .../include/mrm_handler/mrm_handler_core.hpp | 12 +- .../schema/mrm_handler.schema.json | 12 + .../src/mrm_handler/mrm_handler_core.cpp | 143 +++---- 5 files changed, 95 insertions(+), 434 deletions(-) diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index 292d459f69d88..f712c242fe854 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -4,6 +4,8 @@ ros__parameters: update_rate: 10 timeout_operation_mode_availability: 0.5 + timeout_call_mrm_behavior: 0.01 + timeout_cancel_mrm_behavior: 0.01 use_emergency_holding: false timeout_emergency_recovery: 5.0 timeout_takeover_request: 10.0 diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg index 13a2956b6f15c..3c93c8415ef0b 100644 --- a/system/mrm_handler/image/mrm-state.svg +++ b/system/mrm_handler/image/mrm-state.svg @@ -1,362 +1,4 @@ - - - - - - - - -
-
-
- not emergency -
-
-
- - not emergency - - - - - - - - - - - -
-
-
- emergency -
-
-
- - emergency - - - - - - -
-
-
- NORMAL -
-
-
- - NORMAL - - - - - - - -
-
-
- succeeded -
-
-
- - succeeded - - - - - - - -
-
-
- failed -
-
-
- - failed - - - - - - - - -
-
-
- MRM_OPERATING -
-
-
- - MRM_OPERATING - - - - - - - -
-
-
- pull over unavailable -
- comfortable_stop available (ca) -
-
-
- - pull over unavailable... - - - - - - - -
-
-
- pull over available (pa) -
-
-
- - pull over available (pa) - - - - - - - -
-
-
- pull over unavailable -
- comfortable_stop unavailable -
- emergency_stop available (ea) -
-
-
- - pull over unavailable... - - - - - - - - - - - -
-
-
- pull_over -
-
-
- - pull_over - - - - - - - -
-
-
- ea -
-
-
- - ea - - - - - -
-
-
- ea -
-
-
- - ea - - - - - - -
-
-
- emergency_stop -
-
-
- - emergency_stop - - - - - - - -
-
-
- ca -
-
-
- - ca - - - - - - -
-
-
- comfortable_stop -
-
-
- - comfortable_stop - - - - - - - - - -
-
-
- pa -
-
-
- - pa - - - - - - -
-
-
- MRM_SUCCEEDED -
-
-
- - MRM_SUCCEEDED - - - - - - -
-
-
- MRM_FAILED -
-
-
- - MRM_FAILED - - - - - - - Text is not SVG - cannot display - - - +
MRM_OPERATING
MRM_OPERATING
pull over available (pa)
pull over available (pa)
pull over unavailable
comfortable_stop unavailable
emergency_stop available (ea)
pull over unavailable...
pull over unavailable
comfortable_stop available (ca)
pull over unavailable...
pull_over
pull_over
ea || failed
ea || failed
ea || failed
ea || failed
emergency_stop
emergency_stop
ca
ca
comfortable_stop
comfortable_stop
not emergency
not emergency
emergency
emergency
NORMAL
NORMAL
succeeded
succeeded
failed
failed
pa
pa
MRM_SUCCEEDED
MRM_SUCCEEDED
MRM_FAILED
MRM_FAILEDText is not SVG - cannot display \ No newline at end of file diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 8a6ec81cbafff..b1d4b0203f138 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -38,6 +38,9 @@ #include #include +#define CALL true +#define CANCEL false + struct HazardLampPolicy { bool emergency; @@ -47,6 +50,8 @@ struct Param { int update_rate; double timeout_operation_mode_availability; + double timeout_call_mrm_behavior; + double timeout_cancel_mrm_behavior; bool use_emergency_holding; double timeout_emergency_recovery; double timeout_takeover_request; @@ -123,10 +128,8 @@ class MrmHandler : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; rclcpp::Client::SharedPtr client_mrm_emergency_stop_; - void callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + bool requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; @@ -151,6 +154,7 @@ class MrmHandler : public rclcpp::Node void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); + void handlePostFailureRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index 86b18449cb734..6651db8262dde 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -16,6 +16,16 @@ "description": "If the input `operation_mode_availability` topic cannot be received for more than `timeout_operation_mode_availability`, vehicle will make an emergency stop.", "default": 0.5 }, + "timeout_call_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_call_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, + "timeout_cancel_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_cancel_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, "use_emergency_holding": { "type": "boolean", "description": "If this parameter is true, the handler does not recover to the NORMAL state.", @@ -66,6 +76,8 @@ "required": [ "update_rate", "timeout_operation_mode_availability", + "timeout_call_mrm_behavior", + "timeout_cancel_mrm_behavior", "use_emergency_holding", "timeout_emergency_recovery", "timeout_takeover_request", diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 976f5b3164abd..76a4f025fb4c5 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -22,8 +22,9 @@ MrmHandler::MrmHandler() : Node("mrm_handler") { // Parameter param_.update_rate = declare_parameter("update_rate", 10); - param_.timeout_operation_mode_availability = - declare_parameter("timeout_operation_mode_availability", 0.5); + param_.timeout_operation_mode_availability = declare_parameter("timeout_operation_mode_availability", 0.5); + param_.timeout_call_mrm_behavior = declare_parameter("timeout_call_mrm_behavior", 0.01); + param_.timeout_cancel_mrm_behavior = declare_parameter("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); @@ -220,18 +221,27 @@ void MrmHandler::operateMrm() if (mrm_state_.state == MrmState::NORMAL) { // Cancel MRM behavior when returning to NORMAL state const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (requestMrmBehavior(mrm_state_.behavior, CANCEL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handlePostFailureRequest(); } return; } if (mrm_state_.state == MrmState::MRM_OPERATING) { const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - callMrmBehavior(current_mrm_behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (!requestMrmBehavior(mrm_state_.behavior, CANCEL)) { + handlePostFailureRequest(); + } else if (requestMrmBehavior(current_mrm_behavior, CALL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handlePostFailureRequest(); } return; } @@ -247,88 +257,79 @@ void MrmHandler::operateMrm() RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); } -void MrmHandler::callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +void MrmHandler::handlePostFailureRequest() { using autoware_adapi_v1_msgs::msg::MrmState; - auto request = std::make_shared(); - request->operate = true; - - if (mrm_behavior == MrmState::NONE) { - RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); - return; - } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to operate"); - } - return; + if(requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { + if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); + } else { + transitionTo(MrmState::MRM_FAILED); } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); + mrm_state_.behavior = MrmState::EMERGENCY_STOP; } -void MrmHandler::cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +bool MrmHandler::requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const { using autoware_adapi_v1_msgs::msg::MrmState; auto request = std::make_shared(); - request->operate = false; + request->operate = call_or_cancel; // true: call, false: cancel + const auto duration = std::chrono::duration>(call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + std::shared_future> future; - if (mrm_behavior == MrmState::NONE) { - // Do nothing - return; - } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to cancel"); + const auto behavior2string = [](const int behavior) { + if (behavior == MrmState::NONE) { + return "NONE"; } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to cancel"); + if (behavior == MrmState::PULL_OVER) { + return "PULL_OVER"; } - return; + if (behavior == MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + if (behavior == MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + const auto msg = "invalid behavior: " + std::to_string(behavior); + throw std::runtime_error(msg); + }; + + switch (mrm_behavior) { + case MrmState::NONE: + RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); + return true; + case MrmState::PULL_OVER: { + future = client_mrm_pull_over_->async_send_request(request).future.share(); + break; + } + case MrmState::COMFORTABLE_STOP: { + future = client_mrm_comfortable_stop_->async_send_request(request).future.share(); + break; + } + case MrmState::EMERGENCY_STOP: { + future = client_mrm_emergency_stop_->async_send_request(request).future.share(); + break; + } + default: + RCLCPP_ERROR(this->get_logger(), "invalid behavior: %d", mrm_behavior); + return false; } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + + if(future.wait_for(duration) == std::future_status::ready) { + const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); + RCLCPP_WARN(this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.", behavior2string(mrm_behavior)); + return true; } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to cancel"); + RCLCPP_ERROR(this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.", behavior2string(mrm_behavior)); + return false; } - return; + } else { + RCLCPP_ERROR(this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.", behavior2string(mrm_behavior)); + return false; } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); } bool MrmHandler::isDataReady() From 157681080bc6db90c8cca704a9bb277b687a172a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Mar 2024 17:00:12 +0000 Subject: [PATCH 2/7] style(pre-commit): autofix --- system/mrm_handler/image/mrm-state.svg | 358 +++++++++++++++++- .../include/mrm_handler/mrm_handler_core.hpp | 3 +- .../src/mrm_handler/mrm_handler_core.cpp | 28 +- 3 files changed, 378 insertions(+), 11 deletions(-) diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg index 3c93c8415ef0b..dbcc13918feff 100644 --- a/system/mrm_handler/image/mrm-state.svg +++ b/system/mrm_handler/image/mrm-state.svg @@ -1,4 +1,360 @@ -
MRM_OPERATING
MRM_OPERATING
pull over available (pa)
pull over available (pa)
pull over unavailable
comfortable_stop unavailable
emergency_stop available (ea)
pull over unavailable...
pull over unavailable
comfortable_stop available (ca)
pull over unavailable...
pull_over
pull_over
ea || failed
ea || failed
ea || failed
ea || failed
emergency_stop
emergency_stop
ca
ca
comfortable_stop
comfortable_stop
not emergency
not emergency
emergency
emergency
NORMAL
NORMAL
succeeded
succeeded
failed
failed
pa
pa
MRM_SUCCEEDED
MRM_SUCCEEDED
MRM_FAILED
MRM_FAILEDText is not SVG - cannot display \ No newline at end of file + + + + + + + + + + +
+
+
+ MRM_OPERATING +
+
+
+ + MRM_OPERATING + + + + + + + +
+
+
+ pull over available (pa) +
+
+
+ + pull over available (pa) + + + + + + + +
+
+
+ pull over unavailable +
+ comfortable_stop unavailable +
+ emergency_stop available (ea) +
+
+
+ + pull over unavailable... + + + + + + + +
+
+
+ pull over unavailable +
+ comfortable_stop available (ca) +
+
+
+ + pull over unavailable... + + + + + + + + + +
+
+
+ pull_over +
+
+
+ + pull_over + + + + + + + +
+
+
+ ea || failed +
+
+
+ + ea || failed + + + + + +
+
+
+ ea || failed +
+
+
+ + ea || failed + + + + + + +
+
+
+ emergency_stop +
+
+
+ + emergency_stop + + + + + + + +
+
+
+ ca +
+
+
+ + ca + + + + + + +
+
+
+ comfortable_stop +
+
+
+ + comfortable_stop + + + + + + + +
+
+
+ not emergency +
+
+
+ + not emergency + + + + + + + + + + +
+
+
+ emergency +
+
+
+ + emergency + + + + + + +
+
+
+ NORMAL +
+
+
+ + NORMAL + + + + + + + +
+
+
+ succeeded +
+
+
+ + succeeded + + + + + + + +
+
+
+ failed +
+
+
+ + failed + + + + + + + + + +
+
+
+ pa +
+
+
+ + pa + + + + + + +
+
+
+ MRM_SUCCEEDED +
+
+
+ + MRM_SUCCEEDED + + + + + + +
+
+
+ MRM_FAILED +
+
+
+ + MRM_FAILED + + + + + + + Text is not SVG - cannot display + + + diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index b1d4b0203f138..052445435fbe4 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -129,7 +129,8 @@ class MrmHandler : public rclcpp::Node rclcpp::Client::SharedPtr client_mrm_emergency_stop_; bool requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const; + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + bool call_or_cancel) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 76a4f025fb4c5..b4ece4dc6303a 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -22,9 +22,11 @@ MrmHandler::MrmHandler() : Node("mrm_handler") { // Parameter param_.update_rate = declare_parameter("update_rate", 10); - param_.timeout_operation_mode_availability = declare_parameter("timeout_operation_mode_availability", 0.5); + param_.timeout_operation_mode_availability = + declare_parameter("timeout_operation_mode_availability", 0.5); param_.timeout_call_mrm_behavior = declare_parameter("timeout_call_mrm_behavior", 0.01); - param_.timeout_cancel_mrm_behavior = declare_parameter("timeout_cancel_mrm_behavior", 0.01); + param_.timeout_cancel_mrm_behavior = + declare_parameter("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); @@ -261,7 +263,7 @@ void MrmHandler::handlePostFailureRequest() { using autoware_adapi_v1_msgs::msg::MrmState; - if(requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { + if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); } else { transitionTo(MrmState::MRM_FAILED); @@ -270,13 +272,15 @@ void MrmHandler::handlePostFailureRequest() } bool MrmHandler::requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + bool call_or_cancel) const { using autoware_adapi_v1_msgs::msg::MrmState; auto request = std::make_shared(); request->operate = call_or_cancel; // true: call, false: cancel - const auto duration = std::chrono::duration>(call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + const auto duration = std::chrono::duration>( + call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); std::shared_future> future; const auto behavior2string = [](const int behavior) { @@ -317,17 +321,23 @@ bool MrmHandler::requestMrmBehavior( return false; } - if(future.wait_for(duration) == std::future_status::ready) { + if (future.wait_for(duration) == std::future_status::ready) { const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.", behavior2string(mrm_behavior)); + RCLCPP_WARN( + this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.", + behavior2string(mrm_behavior)); return true; } else { - RCLCPP_ERROR(this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.", behavior2string(mrm_behavior)); + RCLCPP_ERROR( + this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.", + behavior2string(mrm_behavior)); return false; } } else { - RCLCPP_ERROR(this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.", behavior2string(mrm_behavior)); + RCLCPP_ERROR( + this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.", + behavior2string(mrm_behavior)); return false; } } From e5ce69be3f1c3b94b3bcabb8ee3d85410b3b1254 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 4 Mar 2024 13:44:25 +0900 Subject: [PATCH 3/7] feat: replace define with enum Signed-off-by: TetsuKawa --- .../include/mrm_handler/mrm_handler_core.hpp | 12 ++++--- .../src/mrm_handler/mrm_handler_core.cpp | 33 +++++++++---------- 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 052445435fbe4..b5458f7923ac2 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -38,8 +38,6 @@ #include #include -#define CALL true -#define CANCEL false struct HazardLampPolicy { @@ -68,6 +66,13 @@ class MrmHandler : public rclcpp::Node MrmHandler(); private: + //type + enum RequestType + { + CALL, + CANCEL + }; + // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr @@ -129,8 +134,7 @@ class MrmHandler : public rclcpp::Node rclcpp::Client::SharedPtr client_mrm_emergency_stop_; bool requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, - bool call_or_cancel) const; + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index b4ece4dc6303a..7286f08cad39d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -226,7 +226,7 @@ void MrmHandler::operateMrm() if (current_mrm_behavior == mrm_state_.behavior) { return; } - if (requestMrmBehavior(mrm_state_.behavior, CANCEL)) { + if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { mrm_state_.behavior = current_mrm_behavior; } else { handlePostFailureRequest(); @@ -238,9 +238,9 @@ void MrmHandler::operateMrm() if (current_mrm_behavior == mrm_state_.behavior) { return; } - if (!requestMrmBehavior(mrm_state_.behavior, CANCEL)) { + if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { handlePostFailureRequest(); - } else if (requestMrmBehavior(current_mrm_behavior, CALL)) { + } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { mrm_state_.behavior = current_mrm_behavior; } else { handlePostFailureRequest(); @@ -272,15 +272,20 @@ void MrmHandler::handlePostFailureRequest() } bool MrmHandler::requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, - bool call_or_cancel) const + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const { using autoware_adapi_v1_msgs::msg::MrmState; auto request = std::make_shared(); - request->operate = call_or_cancel; // true: call, false: cancel - const auto duration = std::chrono::duration>( - call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + if (request_type == RequestType::CALL) { + request->operate = true; + } else if (request_type == RequestType::CANCEL) { + request->operate = false; + } else { + RCLCPP_ERROR(this->get_logger(), "invalid request type: %d", request_type); + return false; + } + const auto duration = std::chrono::duration>(request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); std::shared_future> future; const auto behavior2string = [](const int behavior) { @@ -324,20 +329,14 @@ bool MrmHandler::requestMrmBehavior( if (future.wait_for(duration) == std::future_status::ready) { const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN( - this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.", - behavior2string(mrm_behavior)); + RCLCPP_WARN(this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", behavior2string(mrm_behavior)); return true; } else { - RCLCPP_ERROR( - this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.", - behavior2string(mrm_behavior)); + RCLCPP_ERROR(this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", behavior2string(mrm_behavior)); return false; } } else { - RCLCPP_ERROR( - this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.", - behavior2string(mrm_behavior)); + RCLCPP_ERROR(this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", behavior2string(mrm_behavior)); return false; } } From 1ce4a39b740e4a67ded9d25c99e51b01b8fdce5b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 4 Mar 2024 05:01:19 +0000 Subject: [PATCH 4/7] style(pre-commit): autofix --- .../include/mrm_handler/mrm_handler_core.hpp | 12 ++++-------- .../src/mrm_handler/mrm_handler_core.cpp | 18 +++++++++++++----- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index b5458f7923ac2..1b1073eec08df 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -38,7 +38,6 @@ #include #include - struct HazardLampPolicy { bool emergency; @@ -66,12 +65,8 @@ class MrmHandler : public rclcpp::Node MrmHandler(); private: - //type - enum RequestType - { - CALL, - CANCEL - }; + // type + enum RequestType { CALL, CANCEL }; // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; @@ -134,7 +129,8 @@ class MrmHandler : public rclcpp::Node rclcpp::Client::SharedPtr client_mrm_emergency_stop_; bool requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const; + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 7286f08cad39d..5453c035a6839 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -272,7 +272,8 @@ void MrmHandler::handlePostFailureRequest() } bool MrmHandler::requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const { using autoware_adapi_v1_msgs::msg::MrmState; @@ -285,7 +286,8 @@ bool MrmHandler::requestMrmBehavior( RCLCPP_ERROR(this->get_logger(), "invalid request type: %d", request_type); return false; } - const auto duration = std::chrono::duration>(request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + const auto duration = std::chrono::duration>( + request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); std::shared_future> future; const auto behavior2string = [](const int behavior) { @@ -329,14 +331,20 @@ bool MrmHandler::requestMrmBehavior( if (future.wait_for(duration) == std::future_status::ready) { const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", behavior2string(mrm_behavior)); + RCLCPP_WARN( + this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", + behavior2string(mrm_behavior)); return true; } else { - RCLCPP_ERROR(this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", behavior2string(mrm_behavior)); + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", + behavior2string(mrm_behavior)); return false; } } else { - RCLCPP_ERROR(this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", behavior2string(mrm_behavior)); + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", + behavior2string(mrm_behavior)); return false; } } From f75b8dd00743dbb330b598750b0ca38a48b80bcc Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 4 Mar 2024 14:12:45 +0900 Subject: [PATCH 5/7] modify: renam a function Signed-off-by: TetsuKawa --- .../mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 2 +- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 1b1073eec08df..0fd651d07812a 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -155,7 +155,7 @@ class MrmHandler : public rclcpp::Node void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); - void handlePostFailureRequest(); + void handleFailureRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 5453c035a6839..a52835bad6c99 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -229,7 +229,7 @@ void MrmHandler::operateMrm() if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { mrm_state_.behavior = current_mrm_behavior; } else { - handlePostFailureRequest(); + handleFailureRequest(); } return; } @@ -239,11 +239,11 @@ void MrmHandler::operateMrm() return; } if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { - handlePostFailureRequest(); + handleFailureRequest(); } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { mrm_state_.behavior = current_mrm_behavior; } else { - handlePostFailureRequest(); + handleFailureRequest(); } return; } @@ -259,7 +259,7 @@ void MrmHandler::operateMrm() RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); } -void MrmHandler::handlePostFailureRequest() +void MrmHandler::handleFailureRequest() { using autoware_adapi_v1_msgs::msg::MrmState; From 56225e0bf950666218c5aececc92e93531e61420 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 6 Mar 2024 10:46:45 +0900 Subject: [PATCH 6/7] modify: rename a function Signed-off-by: TetsuKawa --- system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 2 +- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 0fd651d07812a..0eb2fbcf55beb 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -155,7 +155,7 @@ class MrmHandler : public rclcpp::Node void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); - void handleFailureRequest(); + void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index a52835bad6c99..e689e251f754f 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -259,7 +259,7 @@ void MrmHandler::operateMrm() RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); } -void MrmHandler::handleFailureRequest() +void MrmHandler::handleFailedRequest() { using autoware_adapi_v1_msgs::msg::MrmState; From 3c8855acaf2155fb89a1e1e32922b5721793958e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 8 Mar 2024 10:48:14 +0900 Subject: [PATCH 7/7] modify: fix functions name at the caller side Signed-off-by: TetsuKawa --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 724ec753f051c..fccc0cf4f3031 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -218,7 +218,7 @@ void MrmHandler::operateMrm() if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { mrm_state_.behavior = current_mrm_behavior; } else { - handleFailureRequest(); + handleFailedRequest(); } return; } @@ -228,11 +228,11 @@ void MrmHandler::operateMrm() return; } if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { - handleFailureRequest(); + handleFailedRequest(); } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { mrm_state_.behavior = current_mrm_behavior; } else { - handleFailureRequest(); + handleFailedRequest(); } return; }