feat(planning_error_monitor): read params from yaml (tier4#792)

* feat(planning_error_monitor): read params from yaml Signed-off-by: Kazuki Matsumoto <kazuki.matsumoto@tier4.jp> * Update planning/planning_error_monitor/CMakeLists.txt Signed-off-by: Kazuki Matsumoto <kazuki.matsumoto@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * fix: arg line in launch Signed-off-by: Kazuki Matsumoto <kazuki.matsumoto@tier4.jp> * fix: add units of some parameters in yaml Signed-off-by: Kazuki Matsumoto <kazuki.matsumoto@tier4.jp> Co-authored-by: Kazuki Matsumoto <kazuki.matsumoto@tier.jp>
boyali · Sep 28, 2022 · f43d18b · f43d18b
1 parent ab33c62
commit f43d18b
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 4 deletions.
diff --git a/planning/planning_error_monitor/CMakeLists.txt b/planning/planning_error_monitor/CMakeLists.txt
@@ -58,5 +58,6 @@ endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE
+    config
     launch
 )
diff --git a/planning/planning_error_monitor/config/planning_error_monitor.param.yaml b/planning/planning_error_monitor/config/planning_error_monitor.param.yaml
@@ -0,0 +1,7 @@
+/**:
+  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/planning_error_monitor/launch/planning_error_monitor.launch.xml b/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml
@@ -5,11 +5,11 @@
   <let name="input/trajectory" value="/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory" if="$(eval &quot;'$(var run_mode)'=='avoidance'&quot;)"/>
   <let name="input/trajectory" value="/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/trajectory" if="$(eval &quot;'$(var run_mode)'=='surround_obstacle'&quot;)"/>
   <let name="input/trajectory" value="/planning/scenario_planning/lane_driving/trajectory" if="$(eval &quot;'$(var run_mode)'=='obstacle_stop'&quot;)"/>
+  <arg name="planning_error_monitor_param_path" default="$(find-pkg-share planning_error_monitor)/config/planning_error_monitor.param.yaml"/>
   <node name="planning_error_monitor" exec="planning_error_monitor" pkg="planning_error_monitor" output="screen">
+    <!-- load config a file -->
+    <param from="$(var planning_error_monitor_param_path)"/>
+    <!-- remap topic name -->
     <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
-    <param name="error_interval" value="100.0"/>
-    <param name="error_curvature" value="2.0"/>
-    <param name="error_sharp_angle" value="0.785398"/>
-    <param name="ignore_too_close_points" value="0.01"/>
   </node>
 </launch>
-Original file line number
+Diff line change
@@ Expand Up / @@ -58,5 +58,6 @@ endif() @@
     ament_auto_package(
       INSTALL_TO_SHARE
+        config
         launch
     )