From 4b26093ee6ffae324a06dbe9d7c39b3b4389d2f2 Mon Sep 17 00:00:00 2001 From: SHtokuda <165623782+shtokuda@users.noreply.github.com> Date: Thu, 11 Jul 2024 11:28:18 +0900 Subject: [PATCH] fix(autoware_external_cmd_converter): fix check_topic_state (#7921) * fix(autoware_external_cmd_converter): fix check_topic_state Signed-off-by: shtokuda * style(pre-commit): autofix * Update vehicle/autoware_external_cmd_converter/src/node.cpp Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> * style(pre-commit): autofix --------- Signed-off-by: shtokuda Co-authored-by: shtokuda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../autoware_external_cmd_converter/src/node.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/vehicle/autoware_external_cmd_converter/src/node.cpp b/vehicle/autoware_external_cmd_converter/src/node.cpp index cbff8ab5ef2ce..22fc1c319a4e3 100644 --- a/vehicle/autoware_external_cmd_converter/src/node.cpp +++ b/vehicle/autoware_external_cmd_converter/src/node.cpp @@ -179,6 +179,9 @@ void ExternalCmdConverterNode::check_topic_status( { using diagnostic_msgs::msg::DiagnosticStatus; DiagnosticStatus status; + + current_gate_mode_ = gate_mode_sub_.takeData(); + if (!check_emergency_stop_topic_timeout()) { status.level = DiagnosticStatus::ERROR; status.message = "emergency stop topic is timeout"; @@ -195,6 +198,14 @@ void ExternalCmdConverterNode::check_topic_status( bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout() { + if (!current_gate_mode_) { + return true; + } + + if (current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO) { + latest_emergency_stop_heartbeat_received_time_ = nullptr; + } + if (!latest_emergency_stop_heartbeat_received_time_) { return wait_for_first_topic_; } @@ -205,8 +216,6 @@ bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout() bool ExternalCmdConverterNode::check_remote_topic_rate() { - current_gate_mode_ = gate_mode_sub_.takeData(); - if (!current_gate_mode_) { return true; }