From f7837b88297a361347b915cb0198b0aa5283e068 Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Mon, 2 Jan 2023 21:19:00 +0900
Subject: [PATCH] feat(planning_validator): add planning_validator pkg

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 .../planning_error_monitor.param.yaml            |  7 -------
 .../planning_validator.param.yaml                | 16 ++++++++++++++++
 planning_launch/launch/planning.launch.xml       |  6 ++++--
 .../scenario_planning.launch.xml                 |  2 +-
 4 files changed, 21 insertions(+), 10 deletions(-)
 delete mode 100644 planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml
 create mode 100644 planning_launch/config/planning_validator/planning_validator.param.yaml

diff --git a/planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml b/planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml
deleted file mode 100644
index d52f24553..000000000
--- a/planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-/**:
-  ros__parameters:
-    # trajectory check
-    error_interval: 100.0 # error interval distance threshold [m]
-    error_curvature: 2.0 # error curvature threshold [rad/m]
-    error_sharp_angle: 0.785398 # error sharp angle threshold [rad]
-    ignore_too_close_points: 0.01 # ignore too close distance threshold [m]
diff --git a/planning_launch/config/planning_validator/planning_validator.param.yaml b/planning_launch/config/planning_validator/planning_validator.param.yaml
new file mode 100644
index 000000000..16be56c5b
--- /dev/null
+++ b/planning_launch/config/planning_validator/planning_validator.param.yaml
@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    publish_diag: true  # if true, diagnostic msg is published
+    use_previous_trajectory_on_invalid: true # if true, invalid trajectory is not published, previous valid trajectory is published instead.
+    display_on_terminal: true # show error msg on terminal
+    thresholds:
+      interval: 100.0
+      relative_angle: 2.0  # (= 115 degree)
+      curvature: 1.0
+      lateral_acc: 9.8
+      longitudinal_max_acc: 9.8
+      longitudinal_min_acc: -9.8
+      steering: 1.414
+      steering_rate: 10.0
+      velocity_deviation: 100.0
+      distance_deviation: 100.0
diff --git a/planning_launch/launch/planning.launch.xml b/planning_launch/launch/planning.launch.xml
index 8116564f8..f7e1a1e76 100644
--- a/planning_launch/launch/planning.launch.xml
+++ b/planning_launch/launch/planning.launch.xml
@@ -26,8 +26,10 @@
 
     <!-- planning error monitor -->
     <group>
-      <push-ros-namespace namespace="planning_diagnostics"/>
-      <include file="$(find-pkg-share planning_launch)/launch/planning_diagnostics/planning_error_monitor.launch.xml">
+      <include file="$(find-pkg-share planning_validator)/launch/planning_validator.launch.xml">
+        <arg name="planning_validator_param_path" value="$(find-pkg-share planning_launch)/config/planning_validator/planning_validator.param.yaml" />
+        <arg name="input_trajectory" value="/planning/scenario_planning/motion_velocity_smoother/trajectory" />
+        <arg name="output_trajectory" value="/planning/scenario_planning/trajectory" />
       </include>
     </group>
   </group>
diff --git a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index fc06076d5..220c59e8e 100644
--- a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -31,7 +31,7 @@
     <!-- motion velocity smoother -->
     <arg name="smoother_type" default="JerkFiltered" description="options: JerkFiltered, L2, Analytical, Linf(Unstable)"/>
     <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory" />
-    <set_remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory" />
+    <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory" />
     <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
       <arg name="smoother_type" value="$(var smoother_type)" />
       <arg name="common_param_path" value="$(var common_param_path)"/>