From f5e0af7e5fa886badba3f6d88c3f2844477c6d59 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 5 Jun 2024 09:57:07 +0900 Subject: [PATCH 1/4] feat(mrm_handler): mrm recoverable option (#1316) * implement is_mrm_recoverable option of mrm_handler * add pull over after stopped option * update json schema of mrm_handler Signed-off-by: Tomohito Ando --- system/mrm_handler/config/mrm_handler.param.yaml | 2 ++ .../include/mrm_handler/mrm_handler_core.hpp | 3 +++ system/mrm_handler/schema/mrm_handler.schema.json | 12 ++++++++++++ .../mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 12 +++++++++--- 4 files changed, 26 insertions(+), 3 deletions(-) diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index e82ee36a7825a..a5ccf7add987e 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -8,8 +8,10 @@ timeout_cancel_mrm_behavior: 0.01 use_emergency_holding: false timeout_emergency_recovery: 5.0 + is_mrm_recoverable: true use_parking_after_stopped: false use_pull_over: false + use_pull_over_after_stopped: false use_comfortable_stop: false # setting whether to turn hazard lamp on for each situation 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 f73d0df4153ce..b2d835778e93e 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -53,8 +53,10 @@ struct Param double timeout_cancel_mrm_behavior; bool use_emergency_holding; double timeout_emergency_recovery; + bool is_mrm_recoverable; bool use_parking_after_stopped; bool use_pull_over; + bool use_pull_over_after_stopped; bool use_comfortable_stop; HazardLampPolicy turning_hazard_on{}; }; @@ -142,6 +144,7 @@ class MrmHandler : public rclcpp::Node // Algorithm bool is_emergency_holding_ = false; uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; + bool is_mrm_holding_ = false; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index 9a50c6a326f01..75ab13cd913d9 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -36,6 +36,11 @@ "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.", "default": 5.0 }, + "is_mrm_recoverable": { + "type": "boolean", + "description": "If this parameter is true, mrm state never return to normal state", + "default": false + }, "use_parking_after_stopped": { "type": "boolean", "description": "If this parameter is true, it will publish PARKING shift command.", @@ -46,6 +51,11 @@ "description": "If this parameter is true, operate pull over when latent faults occur.", "default": "false" }, + "use_pull_over_after_stopped": { + "type": "boolean", + "description": "If this parameter is true, pull over can be operated after stopped.", + "default": "true" + }, "use_comfortable_stop": { "type": "boolean", "description": "If this parameter is true, operate comfortable stop when latent faults occur.", @@ -70,8 +80,10 @@ "timeout_cancel_mrm_behavior", "use_emergency_holding", "timeout_emergency_recovery", + "is_mrm_recoverable", "use_parking_after_stopped", "use_pull_over", + "use_pull_over_after_stopped", "use_comfortable_stop", "turning_hazard_on" ], 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 bac932a7d7e1e..e8c057152ad94 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -29,8 +29,11 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" 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_.is_mrm_recoverable = declare_parameter("is_mrm_recoverable", true); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); param_.use_pull_over = declare_parameter("use_pull_over", false); + param_.use_pull_over_after_stopped = + declare_parameter("use_pull_over_after_stopped", false); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", true); @@ -405,6 +408,9 @@ void MrmHandler::updateMrmState() case MrmState::NORMAL: if (is_control_mode_autonomous && is_operation_mode_autonomous) { transitionTo(MrmState::MRM_OPERATING); + if (!param_.is_mrm_recoverable) { + is_mrm_holding_ = true; + } } return; @@ -485,7 +491,7 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (isStopped() && operation_mode_availability_->pull_over) { - if (param_.use_pull_over) { + if (param_.use_pull_over && param_.use_pull_over_after_stopped) { return MrmState::PULL_OVER; } } @@ -504,7 +510,7 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (isStopped() && operation_mode_availability_->pull_over) { - if (param_.use_pull_over) { + if (param_.use_pull_over && param_.use_pull_over_after_stopped) { return MrmState::PULL_OVER; } } @@ -528,7 +534,7 @@ bool MrmHandler::isStopped() bool MrmHandler::isEmergency() const { return !operation_mode_availability_->autonomous || is_emergency_holding_ || - is_operation_mode_availability_timeout; + is_operation_mode_availability_timeout || is_mrm_holding_; } bool MrmHandler::isControlModeAutonomous() From 4ecf5f9c0b077dfbf83d5ed2c2d90928c7de4f44 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 6 Sep 2024 15:28:09 +0900 Subject: [PATCH 2/4] feat: implement service for clear MRM behavior (#1511) * implement service for clear MRM behavior * clang format Signed-off-by: Tomohito Ando --- .../include/mrm_handler/mrm_handler_core.hpp | 7 +++++++ .../src/mrm_handler/mrm_handler_core.cpp | 18 ++++++++++++++++++ 2 files changed, 25 insertions(+) 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 b2d835778e93e..1ea48d8820fc1 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -39,6 +39,7 @@ #include #include +#include struct HazardLampPolicy { @@ -95,6 +96,9 @@ class MrmHandler : public rclcpp::Node void onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); + void onRecoverMrm( + const std_srvs::srv::Trigger::Request::SharedPtr, + const std_srvs::srv::Trigger::Response::SharedPtr response); // Publisher @@ -119,6 +123,9 @@ class MrmHandler : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; rclcpp::Client::SharedPtr client_mrm_emergency_stop_; + // Services + rclcpp::Service::SharedPtr service_recover_mrm_; + bool requestMrmBehavior( const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) 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 e8c057152ad94..aff0adc3c555f 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -69,6 +69,12 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default, client_mrm_emergency_stop_group_); + // Services + service_recover_mrm_ = create_service( + "/system/clear_mrm", + std::bind(&MrmHandler::onRecoverMrm, this, std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default); + // Initialize mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; @@ -582,5 +588,17 @@ bool MrmHandler::isArrivedAtGoal() return state->mode == OperationModeState::STOP; } +void MrmHandler::onRecoverMrm( + const std_srvs::srv::Trigger::Request::SharedPtr, + const std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (!param_.is_mrm_recoverable) { + is_mrm_holding_ = false; + response->success = true; + } else { + response->success = false; + } +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(MrmHandler) From 2aca1c99677624ae6f52984b0c95bd3094960031 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 22 Nov 2024 19:52:25 +0900 Subject: [PATCH 3/4] feat: implement MRM automatic recovery feature Signed-off-by: Tomohito Ando --- .../launch/system.launch.xml | 6 + system/diagnostic_graph_utils/CMakeLists.txt | 6 + system/diagnostic_graph_utils/package.xml | 4 + .../src/node/recovery.cpp | 117 ++++++++++++++++++ .../src/node/recovery.hpp | 75 +++++++++++ 5 files changed, 208 insertions(+) create mode 100644 system/diagnostic_graph_utils/src/node/recovery.cpp create mode 100644 system/diagnostic_graph_utils/src/node/recovery.hpp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 6253dce864ddf..a6e62f9911270 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -22,6 +22,7 @@ + @@ -142,4 +143,9 @@ + + + + + diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt index 0c36964f49237..fdc7281c48023 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/diagnostic_graph_utils/CMakeLists.txt @@ -13,6 +13,7 @@ ament_auto_add_library(${PROJECT_NAME}_tools SHARED src/node/dump.cpp src/node/converter.cpp src/node/logging.cpp + src/node/recovery.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_tools @@ -30,4 +31,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_tools EXECUTABLE logging_node ) +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::RecoveryNode" + EXECUTABLE recovery_node +) + ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml index c5a70363bfecb..b455a631eccef 100644 --- a/system/diagnostic_graph_utils/package.xml +++ b/system/diagnostic_graph_utils/package.xml @@ -10,9 +10,13 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs + autoware_system_msgs + component_interface_utils diagnostic_msgs rclcpp rclcpp_components + std_srvs tier4_system_msgs ament_lint_auto diff --git a/system/diagnostic_graph_utils/src/node/recovery.cpp b/system/diagnostic_graph_utils/src/node/recovery.cpp new file mode 100644 index 0000000000000..95e190ce2cf5b --- /dev/null +++ b/system/diagnostic_graph_utils/src/node/recovery.cpp @@ -0,0 +1,117 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "recovery.hpp" + +#include + +namespace diagnostic_graph_utils +{ + +RecoveryNode::RecoveryNode(const rclcpp::NodeOptions & options) : Node("dump", options) +{ + using std::placeholders::_1; + const auto qos_aw_state = rclcpp::QoS(1); + const auto qos_mrm_state = rclcpp::QoS(1); + + sub_graph_.register_update_callback(std::bind(&RecoveryNode::on_graph_update, this, _1)); + sub_graph_.subscribe(*this, 1); + + const auto callback_aw_state = std::bind(&RecoveryNode::on_aw_state, this, _1); + sub_aw_state_ = + create_subscription("/autoware/state", qos_aw_state, callback_aw_state); + + const auto callback_mrm_state = std::bind(&RecoveryNode::on_mrm_state, this, _1); + sub_mrm_state_ = + create_subscription("/system/fail_safe/mrm_state", qos_mrm_state, callback_mrm_state); + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_clear_mrm_ = create_client( + "/system/clear_mrm", rmw_qos_profile_services_default, callback_group_); + + fatal_error_ = false; + mrm_occur_ = false; + autonomous_available_ = false; + mrm_by_fatal_error_ = false; +} + +void RecoveryNode::on_graph_update(DiagGraph::ConstSharedPtr graph) +{ + for (const auto & node : graph->nodes()) { + if (node->path() == "/autoware/modes/autonomous") { + autonomous_available_ = node->level() == DiagnosticStatus::OK; + } + + // aggregate non-recoverable error + if (node->path() == "/autoware/fatal_error/autonomous_available") { + if (node->level() != DiagnosticStatus::OK) { + fatal_error_ = true; + } else { + fatal_error_ = false; + } + } + } +} + +void RecoveryNode::on_aw_state(const AutowareState::ConstSharedPtr msg) +{ + auto_driving_ = msg->state == AutowareState::DRIVING; +} + +void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg) +{ + // set flag if mrm happened by fatal error + if (msg->state != MrmState::NORMAL && fatal_error_) { + mrm_by_fatal_error_ = true; + } + // reset flag if recoverd (transition from mrm to normal) + if (mrm_occur_ && msg->state == MrmState::NORMAL) { + mrm_by_fatal_error_ = false; + } + mrm_occur_ = msg->state != MrmState::NORMAL; + // 1. Not emergency + // 2. Non-recovoerable MRM have not happened + // 3. on MRM + // 4. on autonomous driving + if (autonomous_available_ && !mrm_by_fatal_error_ && mrm_occur_ && auto_driving_) { + clear_mrm(); + } +} + +void RecoveryNode::clear_mrm() +{ + const auto req = std::make_shared(); + + auto logger = get_logger(); + if (!srv_clear_mrm_->service_is_ready()) { + RCLCPP_ERROR(logger, "MRM clear server is not ready."); + return; + } + RCLCPP_INFO(logger, "Recover MRM automatically."); + auto res = srv_clear_mrm_->async_send_request(req); + std::future_status status = res.wait_for(std::chrono::milliseconds(50)); + if (status == std::future_status::timeout) { + RCLCPP_INFO(logger, "Service timeout"); + return; + } + if (!res.get()->success) { + RCLCPP_INFO(logger, "Recovering MRM failed."); + return; + } + RCLCPP_INFO(logger, "Recovering MRM succeed."); +} + +} // namespace diagnostic_graph_utils + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::RecoveryNode) diff --git a/system/diagnostic_graph_utils/src/node/recovery.hpp b/system/diagnostic_graph_utils/src/node/recovery.hpp new file mode 100644 index 0000000000000..0d01a07d3db77 --- /dev/null +++ b/system/diagnostic_graph_utils/src/node/recovery.hpp @@ -0,0 +1,75 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NODE__RECOVERY_HPP_ +#define NODE__RECOVERY_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include + +// Autoware +#include + +#include +#include +#include +#include + +#include +#include // Use map for sorting keys. +#include +#include +#include + +namespace diagnostic_graph_utils +{ + +class RecoveryNode : public rclcpp::Node +{ +public: + explicit RecoveryNode(const rclcpp::NodeOptions & options); + +private: + using AutowareState = autoware_system_msgs::msg::AutowareState; + using MrmState = autoware_adapi_v1_msgs::msg::MrmState; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + + bool fatal_error_; + bool autonomous_available_; + bool mrm_occur_; + bool auto_driving_; + bool mrm_by_fatal_error_; + DiagGraphSubscription sub_graph_; + rclcpp::Subscription::SharedPtr sub_aw_state_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + + // service + rclcpp::Client::SharedPtr srv_clear_mrm_; + + // callback group for service + rclcpp::CallbackGroup::SharedPtr callback_group_; + + void on_graph_update(DiagGraph::ConstSharedPtr graph); + void on_aw_state(const AutowareState::ConstSharedPtr msg); + void on_mrm_state(const MrmState::ConstSharedPtr msg); + + void clear_mrm(); +}; + +} // namespace diagnostic_graph_utils + +#endif // NODE__RECOVERY_HPP_ From 86d2533397ad0265f856ddaf231792be939e5e00 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 22 Nov 2024 19:57:29 +0900 Subject: [PATCH 4/4] fix typo Signed-off-by: Tomohito Ando --- system/diagnostic_graph_utils/src/node/recovery.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/diagnostic_graph_utils/src/node/recovery.cpp b/system/diagnostic_graph_utils/src/node/recovery.cpp index 95e190ce2cf5b..57b64c0e0b16b 100644 --- a/system/diagnostic_graph_utils/src/node/recovery.cpp +++ b/system/diagnostic_graph_utils/src/node/recovery.cpp @@ -74,13 +74,13 @@ void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg) if (msg->state != MrmState::NORMAL && fatal_error_) { mrm_by_fatal_error_ = true; } - // reset flag if recoverd (transition from mrm to normal) + // reset flag if recovered (transition from mrm to normal) if (mrm_occur_ && msg->state == MrmState::NORMAL) { mrm_by_fatal_error_ = false; } mrm_occur_ = msg->state != MrmState::NORMAL; // 1. Not emergency - // 2. Non-recovoerable MRM have not happened + // 2. Non-recoverable MRM have not happened // 3. on MRM // 4. on autonomous driving if (autonomous_available_ && !mrm_by_fatal_error_ && mrm_occur_ && auto_driving_) {