forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1818 from tier4/feat/cherry-pick-mrm-recovery
feat(mrm_handler): mrm recoverable option
- Loading branch information
Showing
11 changed files
with
238 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
107 changes: 107 additions & 0 deletions
107
system/autoware_diagnostic_graph_utils/src/node/recovery.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,107 @@ | ||
| // 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 <algorithm> | ||
| #include <memory> | ||
|
|
||
| namespace diagnostic_graph_utils | ||
| { | ||
|
|
||
| RecoveryNode::RecoveryNode(const rclcpp::NodeOptions & options) : Node("dump", options) | ||
| { | ||
| using std::placeholders::_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_mrm_state = std::bind(&RecoveryNode::on_mrm_state, this, _1); | ||
| sub_mrm_state_ = | ||
| create_subscription<MrmState>("/system/fail_safe/mrm_state", qos_mrm_state, callback_mrm_state); | ||
| callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); | ||
| srv_clear_mrm_ = create_client<std_srvs::srv::Trigger>( | ||
| "/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_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 | ||
| if (autonomous_available_ && !mrm_by_fatal_error_ && mrm_occur_) { | ||
| clear_mrm(); | ||
| } | ||
| } | ||
|
|
||
| void RecoveryNode::clear_mrm() | ||
| { | ||
| const auto req = std::make_shared<std_srvs::srv::Trigger::Request>(); | ||
|
|
||
| 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_macro.hpp> | ||
| RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::RecoveryNode) |
71 changes: 71 additions & 0 deletions
71
system/autoware_diagnostic_graph_utils/src/node/recovery.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,71 @@ | ||
| // 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 "autoware/diagnostic_graph_utils/subscription.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <diagnostic_msgs/msg/diagnostic_status.hpp> | ||
|
|
||
| // Autoware | ||
| #include <autoware/component_interface_utils/rclcpp.hpp> | ||
|
|
||
| #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp> | ||
| #include <std_msgs/msg/string.hpp> | ||
| #include <std_srvs/srv/trigger.hpp> | ||
|
|
||
| #include <functional> | ||
| #include <map> // Use map for sorting keys. | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| namespace diagnostic_graph_utils | ||
| { | ||
|
|
||
| class RecoveryNode : public rclcpp::Node | ||
| { | ||
| public: | ||
| explicit RecoveryNode(const rclcpp::NodeOptions & options); | ||
|
|
||
| private: | ||
| 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_; | ||
| bool mrm_occur_; | ||
| bool mrm_by_fatal_error_; | ||
| DiagGraphSubscription sub_graph_; | ||
| rclcpp::Subscription<MrmState>::SharedPtr sub_mrm_state_; | ||
|
|
||
| // service | ||
| rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv_clear_mrm_; | ||
|
|
||
| // callback group for service | ||
| rclcpp::CallbackGroup::SharedPtr callback_group_; | ||
|
|
||
| void on_graph_update(DiagGraph::ConstSharedPtr graph); | ||
| void on_mrm_state(const MrmState::ConstSharedPtr msg); | ||
|
|
||
| void clear_mrm(); | ||
| }; | ||
|
|
||
| } // namespace diagnostic_graph_utils | ||
|
|
||
| #endif // NODE__RECOVERY_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters