Merge pull request #1408 from tier4/beta/v0.29.0+autoware_external_cmd

fix(autoware_external_cmd_converter, system_diagnostic_monitor): fix check_topic_state and local mode config
tier4 · Jul 11, 2024 · 58ddfa8 · 58ddfa8
2 parents 65e1d20 + ae2e9b0
commit 58ddfa8
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 13 deletions.
diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/system_diagnostic_monitor/config/control.yaml
@@ -13,14 +13,14 @@ units:
   - path: /autoware/control/local
     type: and
     list:
-      - { type: link, link: /autoware/control/topic_rate_check/selector }
-      - { type: link, link: /autoware/control/topic_rate_check/local }
+      - { type: link, link: /autoware/control/topic_rate_check/external_cmd_selector }
+      - { type: link, link: /autoware/control/topic_rate_check/external_cmd_converter }
 
   - path: /autoware/control/remote
     type: and
     list:
-      - { type: link, link: /autoware/control/topic_rate_check/selector }
-      - { type: link, link: /autoware/control/topic_rate_check/remote }
+      - { type: link, link: /autoware/control/topic_rate_check/external_cmd_selector }
+      - { type: link, link: /autoware/control/topic_rate_check/external_cmd_converter }
 
   - path: /autoware/control/topic_rate_check/trajectory_follower
     type: diag
@@ -57,17 +57,12 @@ units:
     node: controller_node_exe
     name: control_state
 
-  - path: /autoware/control/topic_rate_check/selector
+  - path: /autoware/control/topic_rate_check/external_cmd_selector
     type: diag
     node: external_cmd_selector
     name: heartbeat
 
-  - path: /autoware/control/topic_rate_check/local
-    type: diag
-    node: joy_controller
-    name: joy_controller_connection
-
-  - path: /autoware/control/topic_rate_check/remote
+  - path: /autoware/control/topic_rate_check/external_cmd_converter
     type: diag
     node: external_cmd_converter
     name: remote_control_topic_status
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;
   }