From 05ece7a16bdc69556926412e6e9b021bc05d1346 Mon Sep 17 00:00:00 2001 From: Karmen Lu Date: Thu, 30 Jun 2022 00:59:10 -0400 Subject: [PATCH] refactor(freespace_planner): parameterize margin. (#1190) Signed-off-by: Karmen Lu --- .../parking/freespace_planner/freespace_planner.param.yaml | 1 + planning/freespace_planner/README.md | 1 + planning/freespace_planner/config/freespace_planner.param.yaml | 1 + .../include/freespace_planner/freespace_planner_node.hpp | 1 + .../src/freespace_planner/freespace_planner_node.cpp | 3 ++- 5 files changed, 6 insertions(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml index 72648a9dfdd5c..d6152c4f56243 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml +++ b/launch/tier4_planning_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 index b7b31baac23f9..5d6febd98be79 100644 --- 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 index 0514ae6ceaa14..4764690a93a9c 100644 --- 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 index 4ce0ca29b9122..d9396175698be 100644 --- 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 index 677f9ede99813..ed6f307802f48 100644 --- 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("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;