From 0c98a010f23c328c4b1c16106151443069918b50 Mon Sep 17 00:00:00 2001
From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Date: Tue, 27 Oct 2020 17:38:13 +0900
Subject: [PATCH] Add vehicle cmd gate config (#136)

* Add config_file of vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add use_emergency_stop

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename emergency_stop to external_emergency_stop

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix command_gate diag

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>
---
 .../config/vehicle_cmd_gate/vehicle_cmd_gate.yaml        | 9 +++++++++
 control_launch/launch/control.launch                     | 4 ++++
 system_launch/config/diagnostic_aggregator/control.yaml  | 2 +-
 3 files changed, 14 insertions(+), 1 deletion(-)
 create mode 100644 control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml

diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml
new file mode 100644
index 000000000..0a9a3e151
--- /dev/null
+++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml
@@ -0,0 +1,9 @@
+update_rate: 10
+system_emergency_heartbeat_timeout: 0.5
+external_emergency_stop_heartbeat_timeout: 0.0
+
+vel_lim: 25.0
+lon_acc_lim: 5.0
+lon_jerk_lim: 5.0
+lat_acc_lim: 5.0
+lat_jerk_lim: 5.0
diff --git a/control_launch/launch/control.launch b/control_launch/launch/control.launch
index 2a4000982..5a3f91252 100644
--- a/control_launch/launch/control.launch
+++ b/control_launch/launch/control.launch
@@ -8,6 +8,7 @@
   <arg name="mpc_follower_param_path" default="$(find control_launch)/config/mpc_follower/mpc_follower_param.yaml"/>
   <arg name="pure_pursuit_param_path" default="$(find control_launch)/config/pure_pursuit/pure_pursuit_param.yaml"/>
   <arg name="velocity_controller_param_path" default="$(find control_launch)/config/velocity_controller/velocity_controller_param.yaml"/>
+  <arg name="vehicle_cmd_gate_param_path" default="$(find control_launch)/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml"/>
 
   <!-- control module -->
   <group ns="control">
@@ -43,6 +44,9 @@
 
     <!-- vehicle cmd gate -->
     <include file="$(find vehicle_cmd_gate)/launch/vehicle_cmd_gate.launch">
+      <arg name="config_file" value="$(arg vehicle_cmd_gate_param_path)"/>
+      <arg name="use_emergency_handling" value="false"/>
+      <arg name="use_external_emergency_stop" value="true"/>
     </include>
 
     <!-- remote_cmd_converter -->
diff --git a/system_launch/config/diagnostic_aggregator/control.yaml b/system_launch/config/diagnostic_aggregator/control.yaml
index 7c519c849..00af404ae 100644
--- a/system_launch/config/diagnostic_aggregator/control.yaml
+++ b/system_launch/config/diagnostic_aggregator/control.yaml
@@ -4,7 +4,7 @@ analyzers:
   command_gate:
     type: diagnostic_aggregator/GenericAnalyzer
     path: command_gate
-    contains: ": command_gate"
+    contains: "vehicle_cmd_gate: heartbeat"
     timeout: 1.0
     num_items: 0