fix(autoware_external_cmd_converter): fix check_topic_state (#7921)

* fix(autoware_external_cmd_converter): fix check_topic_state

Signed-off-by: shtokuda <shumpei.tokuda@tier4.jp>

* 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 <shumpei.tokuda@tier4.jp>
Co-authored-by: shtokuda <shumpei.tokuda@tier4.jp>
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>

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;
   }