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..57b64c0e0b16b --- /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 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/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_ 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..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 { @@ -53,8 +54,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{}; }; @@ -93,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 @@ -117,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; @@ -142,6 +151,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..aff0adc3c555f 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); @@ -66,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; @@ -405,6 +414,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 +497,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 +516,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 +540,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() @@ -576,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)