From 1ab9f6b931d8688ff12d155dfd949e9f6769d77a Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Mon, 2 Jan 2023 21:11:19 +0900
Subject: [PATCH] update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 .../invalid_trajectory_publisher.launch.xml   |  4 +-
 .../launch/planning_validator.launch.xml      |  6 +-
 .../diagnostic_aggregator/planning.param.yaml | 58 ++++++++++++++++---
 3 files changed, 56 insertions(+), 12 deletions(-)

diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml
index 42ef21f3c3cc4..fb64d2e0841bd 100644
--- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml
+++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml
@@ -1,6 +1,6 @@
 <launch>
-  <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
-  <arg name="output/trajectory" default="/planning/scenario_planning/trajectory"/>
+  <arg name="input/trajectory" default="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+  <arg name="output/trajectory" default="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
 
   <node name="invalid_trajectory_publisher" exec="invalid_trajectory_publisher" pkg="planning_validator" output="screen">
     <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml
index 42a47b898c5f8..a6b34edeec68c 100644
--- a/planning/planning_validator/launch/planning_validator.launch.xml
+++ b/planning/planning_validator/launch/planning_validator.launch.xml
@@ -1,13 +1,15 @@
 <launch>
   <arg name="planning_validator_param_path" default="$(find-pkg-share planning_validator)/config/planning_validator.param.yaml"/>
+  <arg name="input_trajectory" default="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+  <arg name="output_trajectory" default="/planning/scenario_planning/trajectory"/>
 
   <node name="planning_validator" exec="planning_validator_node" pkg="planning_validator" output="screen">
     <!-- load config a file -->
     <param from="$(var planning_validator_param_path)"/>
 
     <!-- remap topic name -->
-    <remap from="~/input/trajectory" to="/planning/scenario_planning/trajectory"/>
+    <remap from="~/input/trajectory" to="$(var input_trajectory)"/>
     <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
-    <remap from="~/output/trajectory" to="/planning/trajectory"/>
+    <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
   </node>
 </launch>
diff --git a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml
index 151c6247dbbb4..a13ab3568a5d7 100644
--- a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml
+++ b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml
@@ -24,24 +24,66 @@
               analyzers:
                 trajectory_finite_value:
                   type: diagnostic_aggregator/GenericAnalyzer
-                  path: trajectory_point_validation
-                  contains: [": trajectory_point_validation"]
+                  path: trajectory_validation_finite
+                  contains: [": trajectory_validation_finite"]
                   timeout: 1.0
 
                 trajectory_interval:
                   type: diagnostic_aggregator/GenericAnalyzer
-                  path: trajectory_interval_validation
-                  contains: [": trajectory_interval_validation"]
+                  path: trajectory_validation_interval
+                  contains: [": trajectory_validation_interval"]
                   timeout: 1.0
 
                 trajectory_curvature:
                   type: diagnostic_aggregator/GenericAnalyzer
-                  path: trajectory_curvature_validation
-                  contains: [": trajectory_curvature_validation"]
+                  path: trajectory_validation_curvature
+                  contains: [": trajectory_validation_curvature"]
                   timeout: 1.0
 
                 trajectory_sharp_angle:
                   type: diagnostic_aggregator/GenericAnalyzer
-                  path: trajectory_relative_angle_validation
-                  contains: [": trajectory_relative_angle_validation"]
+                  path: trajectory_validation_relative_angle
+                  contains: [": trajectory_validation_relative_angle"]
                   timeout: 1.0
+
+                trajectory_relative_angle:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_relative_angle
+                  contains: [": trajectory_validation_relative_angle"]
+                  timeout: 1.0
+
+                trajectory_lateral_acceleration:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_lateral_acceleration
+                  contains: [": trajectory_validation_lateral_acceleration"]
+                  timeout: 1.0
+
+                trajectory_acceleration:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_acceleration
+                  contains: [": trajectory_validation_acceleration"]
+                  timeout: 1.0
+
+                trajectory_deceleration:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_deceleration
+                  contains: [": trajectory_validation_deceleration"]
+                  timeout: 1.0
+
+                trajectory_steering:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_steering
+                  contains: [": trajectory_validation_steering"]
+                  timeout: 1.0
+
+                trajectory_steering_rate:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_steering_rate
+                  contains: [": trajectory_validation_steering_rate"]
+                  timeout: 1.0
+
+                trajectory_velocity_deviation:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: trajectory_validation_velocity_deviation
+                  contains: [": trajectory_validation_velocity_deviation"]
+                  timeout: 1.0
\ No newline at end of file