feat(planning_validator): add planning_validator pkg

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

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

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>

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)"/>