Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(recovery_node): recover from MRM even duaring manual driving #1777

Merged
merged 1 commit into from
Jan 27, 2025
Merged

fix(recovery_node): recover from MRM even duaring manual driving #1777

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 1 addition & 12 deletions system/diagnostic_graph_utils/src/node/recovery.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,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<AutowareState>("/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<MrmState>("/system/fail_safe/mrm_state", qos_mrm_state, callback_mrm_state);
Expand Down Expand Up @@ -63,11 +58,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
Expand All @@ -82,8 +72,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();
}
}
Expand Down
5 changes: 0 additions & 5 deletions system/diagnostic_graph_utils/src/node/recovery.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@
#include <component_interface_utils/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_system_msgs/msg/autoware_state.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_srvs/srv/trigger.hpp>

Expand All @@ -44,17 +43,14 @@ 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;

bool fatal_error_;
bool autonomous_available_;
bool mrm_occur_;
bool auto_driving_;
bool mrm_by_fatal_error_;
DiagGraphSubscription sub_graph_;
rclcpp::Subscription<AutowareState>::SharedPtr sub_aw_state_;
rclcpp::Subscription<MrmState>::SharedPtr sub_mrm_state_;

// service
Expand All @@ -64,7 +60,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();
Expand Down
Loading