From 85c91e21be1c5de2f3467b583c17f6e35e13861b Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 23 Nov 2024 09:36:30 +0900 Subject: [PATCH 1/8] feat(mrm_handler): mrm recoverable option (#1316) (#1654) * 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 * feat: implement service for clear MRM behavior (#1511) * implement service for clear MRM behavior * clang format Signed-off-by: Tomohito Ando * feat: implement MRM automatic recovery feature Signed-off-by: Tomohito Ando * fix typo Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando Co-authored-by: Shohei Sakai Signed-off-by: Tomohito Ando --- .../package.xml | 5 + .../launch/system.launch.xml | 6 + .../CMakeLists.txt | 6 + .../src/node/recovery.cpp | 117 ++++++++++++++++++ .../src/node/recovery.hpp | 75 +++++++++++ .../config/mrm_handler.param.yaml | 2 + .../autoware/mrm_handler/mrm_handler_core.hpp | 10 ++ .../schema/mrm_handler.schema.json | 12 ++ .../src/mrm_handler/mrm_handler_core.cpp | 30 ++++- 9 files changed, 260 insertions(+), 3 deletions(-) create mode 100644 system/autoware_diagnostic_graph_utils/src/node/recovery.cpp create mode 100644 system/autoware_diagnostic_graph_utils/src/node/recovery.hpp diff --git a/common/autoware_component_interface_tools/package.xml b/common/autoware_component_interface_tools/package.xml index 20919e15acd13..58510caa9aa2c 100644 --- a/common/autoware_component_interface_tools/package.xml +++ b/common/autoware_component_interface_tools/package.xml @@ -12,8 +12,13 @@ diagnostic_updater fmt + autoware_adapi_v1_msgs + autoware_system_msgs + component_interface_utils + diagnostic_msgs rclcpp rclcpp_components + std_srvs tier4_system_msgs yaml_cpp_vendor diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index c22b8b091bf59..3b3d0daf46fbe 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -18,6 +18,7 @@ + @@ -122,4 +123,9 @@ + + + + + diff --git a/system/autoware_diagnostic_graph_utils/CMakeLists.txt b/system/autoware_diagnostic_graph_utils/CMakeLists.txt index b1de417b1bc7d..3a6bed920b8ee 100644 --- a/system/autoware_diagnostic_graph_utils/CMakeLists.txt +++ b/system/autoware_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/autoware_diagnostic_graph_utils/src/node/recovery.cpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp new file mode 100644 index 0000000000000..57b64c0e0b16b --- /dev/null +++ b/system/autoware_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 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-recoverable 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/autoware_diagnostic_graph_utils/src/node/recovery.hpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp new file mode 100644 index 0000000000000..0d01a07d3db77 --- /dev/null +++ b/system/autoware_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_ diff --git a/system/autoware_mrm_handler/config/mrm_handler.param.yaml b/system/autoware_mrm_handler/config/mrm_handler.param.yaml index e82ee36a7825a..a5ccf7add987e 100644 --- a/system/autoware_mrm_handler/config/mrm_handler.param.yaml +++ b/system/autoware_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/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp index 8d74db4e5c843..8cf84c8c23454 100644 --- a/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp +++ b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp @@ -40,6 +40,7 @@ #include #include +#include namespace autoware::mrm_handler { @@ -57,8 +58,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{}; }; @@ -97,6 +100,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 @@ -125,6 +131,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; @@ -150,6 +159,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/autoware_mrm_handler/schema/mrm_handler.schema.json b/system/autoware_mrm_handler/schema/mrm_handler.schema.json index 9a50c6a326f01..75ab13cd913d9 100644 --- a/system/autoware_mrm_handler/schema/mrm_handler.schema.json +++ b/system/autoware_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/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp index e9429300de573..18a058fbb6a54 100644 --- a/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -32,8 +32,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); @@ -71,6 +74,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; @@ -415,6 +424,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; @@ -495,7 +507,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; } } @@ -514,7 +526,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; } } @@ -538,7 +550,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() @@ -586,6 +598,18 @@ 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; + } +} + } // namespace autoware::mrm_handler #include From 83765012ea37e1aaa8f71b7a09d441fef1886904 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 14 Feb 2025 15:50:31 +0900 Subject: [PATCH 2/8] fix namespace Signed-off-by: Tomohito Ando --- .../autoware_diagnostic_graph_utils/src/node/recovery.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp index 0d01a07d3db77..1b67af3d403fd 100644 --- a/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp +++ b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp @@ -15,14 +15,14 @@ #ifndef NODE__RECOVERY_HPP_ #define NODE__RECOVERY_HPP_ -#include "diagnostic_graph_utils/subscription.hpp" +#include "autoware/diagnostic_graph_utils/subscription.hpp" #include #include // Autoware -#include +#include #include #include @@ -47,6 +47,8 @@ class RecoveryNode : public rclcpp::Node using AutowareState = autoware_system_msgs::msg::AutowareState; using MrmState = autoware_adapi_v1_msgs::msg::MrmState; using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagGraphSubscription = autoware::diagnostic_graph_utils::DiagGraphSubscription; + using DiagGraph = autoware::diagnostic_graph_utils::DiagGraph; bool fatal_error_; bool autonomous_available_; From f96e5d9e1c8891c958a9bc27785b165bfc0dc710 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 14 Feb 2025 15:51:11 +0900 Subject: [PATCH 3/8] add `autoware_` prefix Signed-off-by: Tomohito Ando --- launch/tier4_system_launch/launch/system.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 3b3d0daf46fbe..7df10e49adba5 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -126,6 +126,6 @@ - + From 1e57a272a83e77bf0a1a06c08d6817eaa077c762 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 14 Feb 2025 15:51:30 +0900 Subject: [PATCH 4/8] add missing packages Signed-off-by: Tomohito Ando --- system/autoware_diagnostic_graph_utils/package.xml | 4 +++- system/autoware_mrm_handler/package.xml | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/system/autoware_diagnostic_graph_utils/package.xml b/system/autoware_diagnostic_graph_utils/package.xml index 527fa1a201e05..b02ad3f82e434 100644 --- a/system/autoware_diagnostic_graph_utils/package.xml +++ b/system/autoware_diagnostic_graph_utils/package.xml @@ -11,11 +11,13 @@ ament_cmake_auto autoware_cmake + autoware_component_interface_utils autoware_internal_debug_msgs + autoware_system_msgs diagnostic_msgs rclcpp rclcpp_components - tier4_system_msgs + std_srvs ament_lint_auto autoware_lint_common diff --git a/system/autoware_mrm_handler/package.xml b/system/autoware_mrm_handler/package.xml index 92cfef1634bc1..e263f31f08425 100644 --- a/system/autoware_mrm_handler/package.xml +++ b/system/autoware_mrm_handler/package.xml @@ -19,6 +19,7 @@ nav_msgs rclcpp rclcpp_components + std_srvs tier4_system_msgs ament_lint_auto From 751058ab49356adfa50531f235cc11b4209358d0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 14 Feb 2025 06:55:22 +0000 Subject: [PATCH 5/8] style(pre-commit): autofix --- common/autoware_component_interface_tools/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_component_interface_tools/package.xml b/common/autoware_component_interface_tools/package.xml index 58510caa9aa2c..62c839bcce5a5 100644 --- a/common/autoware_component_interface_tools/package.xml +++ b/common/autoware_component_interface_tools/package.xml @@ -10,12 +10,12 @@ ament_cmake_auto autoware_cmake - diagnostic_updater - fmt autoware_adapi_v1_msgs autoware_system_msgs component_interface_utils diagnostic_msgs + diagnostic_updater + fmt rclcpp rclcpp_components std_srvs From 3efa72986e77bd777d94c89c3802af70319ca2f7 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 14 Feb 2025 16:58:34 +0900 Subject: [PATCH 6/8] fix pre-commit errors Signed-off-by: Tomohito Ando --- system/autoware_diagnostic_graph_utils/src/node/recovery.cpp | 1 + system/autoware_diagnostic_graph_utils/src/node/recovery.hpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp index 57b64c0e0b16b..11e3a4d6c7e4f 100644 --- a/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp +++ b/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp @@ -15,6 +15,7 @@ #include "recovery.hpp" #include +#include namespace diagnostic_graph_utils { diff --git a/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp index 1b67af3d403fd..11e51bbd7a1b8 100644 --- a/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp +++ b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp @@ -31,7 +31,6 @@ #include #include // Use map for sorting keys. -#include #include #include From 98cd1501fc051aee15e5a27d6408ee6bdbc7ebc2 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 14 Feb 2025 17:47:26 +0900 Subject: [PATCH 7/8] remove unused parameter Signed-off-by: Tomohito Ando --- system/autoware_mrm_handler/config/mrm_handler.param.yaml | 1 - .../include/autoware/mrm_handler/mrm_handler_core.hpp | 1 - system/autoware_mrm_handler/schema/mrm_handler.schema.json | 6 ------ .../src/mrm_handler/mrm_handler_core.cpp | 6 ++---- 4 files changed, 2 insertions(+), 12 deletions(-) diff --git a/system/autoware_mrm_handler/config/mrm_handler.param.yaml b/system/autoware_mrm_handler/config/mrm_handler.param.yaml index a5ccf7add987e..15549d6c157f0 100644 --- a/system/autoware_mrm_handler/config/mrm_handler.param.yaml +++ b/system/autoware_mrm_handler/config/mrm_handler.param.yaml @@ -11,7 +11,6 @@ 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/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp index 8cf84c8c23454..77d10cbc65b27 100644 --- a/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp +++ b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp @@ -61,7 +61,6 @@ struct Param 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{}; }; diff --git a/system/autoware_mrm_handler/schema/mrm_handler.schema.json b/system/autoware_mrm_handler/schema/mrm_handler.schema.json index 75ab13cd913d9..e4e10fbbd6bcd 100644 --- a/system/autoware_mrm_handler/schema/mrm_handler.schema.json +++ b/system/autoware_mrm_handler/schema/mrm_handler.schema.json @@ -51,11 +51,6 @@ "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.", @@ -83,7 +78,6 @@ "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/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 18a058fbb6a54..945ef3c491c7e 100644 --- a/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -35,8 +35,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" 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); @@ -507,7 +505,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 && param_.use_pull_over_after_stopped) { + if (param_.use_pull_over) { return MrmState::PULL_OVER; } } @@ -526,7 +524,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 && param_.use_pull_over_after_stopped) { + if (param_.use_pull_over) { return MrmState::PULL_OVER; } } From 7cbeac7f09839525fa2118034fe6733df6ee9db5 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 27 Jan 2025 09:38:43 +0900 Subject: [PATCH 8/8] fix(recovery_node): recover from MRM even duaring manual driving (#1777) Signed-off-by: Tomohito Ando --- .../src/node/recovery.cpp | 13 +------------ .../src/node/recovery.hpp | 5 ----- 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp index 11e3a4d6c7e4f..c3619a6b2b6d6 100644 --- a/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp +++ b/system/autoware_diagnostic_graph_utils/src/node/recovery.cpp @@ -23,16 +23,11 @@ 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); @@ -64,11 +59,6 @@ void RecoveryNode::on_graph_update(DiagGraph::ConstSharedPtr graph) } } -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 @@ -83,8 +73,7 @@ void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg) // 1. Not emergency // 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_) { + if (autonomous_available_ && !mrm_by_fatal_error_ && mrm_occur_) { clear_mrm(); } } diff --git a/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp index 11e51bbd7a1b8..6f1a0283ba0ed 100644 --- a/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp +++ b/system/autoware_diagnostic_graph_utils/src/node/recovery.hpp @@ -25,7 +25,6 @@ #include #include -#include #include #include @@ -43,7 +42,6 @@ class RecoveryNode : public rclcpp::Node 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; using DiagGraphSubscription = autoware::diagnostic_graph_utils::DiagGraphSubscription; @@ -52,10 +50,8 @@ class RecoveryNode : public rclcpp::Node 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 @@ -65,7 +61,6 @@ class RecoveryNode : public rclcpp::Node 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();