fix(autoware_external_cmd_converter): fix check_topic_state (autoware…

…foundation#7921) * fix(autoware_external_cmd_converter): fix check_topic_state Signed-off-by: shtokuda <[email protected]> * style(pre-commit): autofix * Update vehicle/autoware_external_cmd_converter/src/node.cpp Co-authored-by: Shumpei Wakabayashi <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: shtokuda <[email protected]> Co-authored-by: shtokuda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <[email protected]>
cyn-liu · Jul 11, 2024 · 4b26093 · 4b26093
1 parent 1c7e462
commit 4b26093
Showing 1 changed file with 11 additions and 2 deletions.
diff --git 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;
   }