-
MRM_FAILED
+
MRM_FAILED
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 c706e1d773907..b4a00f0de7af5 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -47,6 +47,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;
bool use_parking_after_stopped;
@@ -61,6 +63,9 @@ class MrmHandler : public rclcpp::Node
MrmHandler();
private:
+ // type
+ enum RequestType { CALL, CANCEL };
+
// Subscribers
rclcpp::Subscription
::SharedPtr sub_odom_;
rclcpp::Subscription::SharedPtr
@@ -120,10 +125,9 @@ 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,
+ RequestType request_type) const;
void logMrmCallingResult(
const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
bool is_call) const;
@@ -146,6 +150,7 @@ class MrmHandler : public rclcpp::Node
void transitionTo(const int new_state);
void updateMrmState();
void operateMrm();
+ void handleFailedRequest();
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 aedb8076ef05c..9a50c6a326f01 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.",
@@ -56,6 +66,8 @@
"required": [
"update_rate",
"timeout_operation_mode_availability",
+ "timeout_call_mrm_behavior",
+ "timeout_cancel_mrm_behavior",
"use_emergency_holding",
"timeout_emergency_recovery",
"use_parking_after_stopped",
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 8ed9017a3ad3b..fccc0cf4f3031 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -24,6 +24,9 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
param_.update_rate = declare_parameter("update_rate", 10);
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_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false);
@@ -209,18 +212,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, RequestType::CANCEL)) {
mrm_state_.behavior = current_mrm_behavior;
+ } else {
+ handleFailedRequest();
}
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, RequestType::CANCEL)) {
+ handleFailedRequest();
+ } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) {
mrm_state_.behavior = current_mrm_behavior;
+ } else {
+ handleFailedRequest();
}
return;
}
@@ -236,88 +248,94 @@ 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::handleFailedRequest()
{
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,
+ RequestType request_type) const
{
using autoware_adapi_v1_msgs::msg::MrmState;
auto request = std::make_shared();
- request->operate = false;
-
- if (mrm_behavior == MrmState::NONE) {
- // Do nothing
- return;
+ 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;
}
- 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 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) {
+ 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(), request->operate ? "%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(), request->operate ? "%s failed to operate." : "%s failed to cancel.",
+ behavior2string(mrm_behavior));
+ return false;
}
- return;
+ } else {
+ RCLCPP_ERROR(
+ this->get_logger(), request->operate ? "%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()