refactor(freespace_planner): parameterize margin. (autowarefoundation…

…#1190) Signed-off-by: Karmen Lu <[email protected]>
tzhong518 · Oct 14, 2022 · 05ece7a · 05ece7a
1 parent 57e1b3e
commit 05ece7a
Show file tree

Hide file tree

Showing 5 changed files with 6 additions and 1 deletion.
diff --git a/...ng_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/...ng_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml
@@ -8,6 +8,7 @@
     th_stopped_time_sec: 1.0
     th_stopped_velocity_mps: 0.01
     th_course_out_distance_m: 1.0
+    vehicle_shape_margin_m: 1.0
     replan_when_obstacle_found: true
     replan_when_course_out: true
 

diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md
@@ -48,6 +48,7 @@ None
 | `th_stopped_time_sec`        | double | threshold time to check if vehicle is stopped                                   |
 | `th_stopped_velocity_mps`    | double | threshold velocity to check if vehicle is stopped                               |
 | `th_course_out_distance_m`   | double | threshold distance to check if vehicle is out of course                         |
+| `vehicle_shape_margin_m`     | double | vehicle margin                                                                  |
 | `replan_when_obstacle_found` | bool   | whether replanning when obstacle has found on the trajectory                    |
 | `replan_when_course_out`     | bool   | whether replanning when vehicle is out of course                                |
 

diff --git a/planning/freespace_planner/config/freespace_planner.param.yaml b/planning/freespace_planner/config/freespace_planner.param.yaml
@@ -8,6 +8,7 @@
     th_stopped_time_sec: 1.0
     th_stopped_velocity_mps: 0.01
     th_course_out_distance_m: 1.0
+    vehicle_shape_margin_m: 1.0
     replan_when_obstacle_found: true
     replan_when_course_out: true
 

diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
@@ -83,6 +83,7 @@ struct NodeParam
   double th_stopped_time_sec;
   double th_stopped_velocity_mps;
   double th_course_out_distance_m;
+  double vehicle_shape_margin_m;
   bool replan_when_obstacle_found;
   bool replan_when_course_out;
 };

diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
@@ -221,6 +221,7 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti
     p.th_stopped_time_sec = declare_parameter("th_stopped_time_sec", 1.0);
     p.th_stopped_velocity_mps = declare_parameter("th_stopped_velocity_mps", 0.01);
     p.th_course_out_distance_m = declare_parameter("th_course_out_distance_m", 3.0);
+    p.vehicle_shape_margin_m = declare_parameter("vehicle_shape_margin_m", 1.0);
     p.replan_when_obstacle_found = declare_parameter("replan_when_obstacle_found", true);
     p.replan_when_course_out = declare_parameter("replan_when_course_out", true);
     declare_parameter<bool>("is_completed", false);
@@ -463,7 +464,7 @@ void FreespacePlannerNode::planTrajectory()
   // Extend robot shape
   freespace_planning_algorithms::VehicleShape extended_vehicle_shape =
     planner_common_param_.vehicle_shape;
-  constexpr double margin = 1.0;
+  const double margin = node_param_.vehicle_shape_margin_m;
   extended_vehicle_shape.length += margin;
   extended_vehicle_shape.width += margin;
   extended_vehicle_shape.base2back += margin / 2;