From 8f6ddcca863bfcb832a0d5cc6c89225d01897e81 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 7 Oct 2022 16:36:22 +0900 Subject: [PATCH 1/2] feat(obstacle_cruise_planner): add an explanation Signed-off-by: yutaka --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- .../config/obstacle_cruise_planner.param.yaml | 2 +- planning/obstacle_cruise_planner/src/planner_interface.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 0c102155d2f00..520637abed775 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -10,7 +10,7 @@ min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] - terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m] + terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 0c102155d2f00..520637abed775 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -10,7 +10,7 @@ min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] - terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m] + terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 4ebef793bc733..e73d83e749139 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -155,7 +155,7 @@ Trajectory PlannerInterface::generateStopTrajectory( abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (stop_dist_diff < 0.5) { + if (stop_dist_diff < longitudinal_info_.safe_distance_margin) { // Use terminal margin (terminal_safe_distance_margin) for obstacle stop return longitudinal_info_.terminal_safe_distance_margin; } From 49d9ea175ca6888e5a22c51411e06c0be94c05bf Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 7 Oct 2022 16:49:58 +0900 Subject: [PATCH 2/2] update readme Signed-off-by: yutaka --- planning/obstacle_cruise_planner/README.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index d4e66d87b5a5a..4d114c47bfaf0 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -149,15 +149,17 @@ In the `obstacle_filtering` namespace, ### Stop planning -| Parameter | Type | Description | -| ----------------------------- | ------ | ----------------------------------------- | -| `common.min_strong_accel` | double | ego's minimum acceleration to stop [m/ss] | -| `common.safe_distance_margin` | double | distance with obstacles for stop [m] | +| Parameter | Type | Description | +| -------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | +| `common.min_strong_accel` | double | ego's minimum acceleration to stop [m/ss] | +| `common.safe_distance_margin` | double | distance with obstacles for stop [m] | +| `common.terminal_safe_distance_margin` | double | terminal_distance with obstacles for stop, which cannot be exceed safe distance margin [m] | The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects. The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles inside the detection area. The safe distance is parameterized as `common.safe_distance_margin`. +When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes `terminal_safe_distance_margin`. When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated. If the acceleration is less than `common.min_strong_accel`, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency.