feat(obstacle_stop_planner): add margin behind goal (autowarefoundati…

…on#2953) * feat(obstacle_stop_planner): add margin behind goal Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * refactor: just compare end index Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1222-takeshi · Mar 6, 2023 · ed1bd46 · ed1bd46
1 parent 5a1e224
commit ed1bd46
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 9 deletions.
diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md
@@ -89,11 +89,12 @@ This module has parameter `hold_stop_margin_distance` in order to prevent from t
 
 #### Stop position
 
-| Parameter                   | Type   | Description                                                                                                                                    |
-| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- |
-| `max_longitudinal_margin`   | double | margin between obstacle and the ego's front [m]                                                                                                |
-| `min_longitudinal_margin`   | double | if any obstacle exists within `max_longitudinal_margin`, this module set margin as the value of _stop margin_ to `min_longitudinal_margin` [m] |
-| `hold_stop_margin_distance` | double | parameter for restart prevention (See above section) [m]                                                                                       |
+| Parameter                             | Type   | Description                                                                                                                                    |
+| ------------------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- |
+| `max_longitudinal_margin`             | double | margin between obstacle and the ego's front [m]                                                                                                |
+| `max_longitudinal_margin_behind_goal` | double | margin between obstacle and the ego's front when the stop point is behind the goal[m]                                                          |
+| `min_longitudinal_margin`             | double | if any obstacle exists within `max_longitudinal_margin`, this module set margin as the value of _stop margin_ to `min_longitudinal_margin` [m] |
+| `hold_stop_margin_distance`           | double | parameter for restart prevention (See above section) [m]                                                                                       |
 
 #### Obstacle detection area
 

diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
@@ -14,9 +14,10 @@
     stop_planner:
       # params for stop position
       stop_position:
-        max_longitudinal_margin: 5.0         # stop margin distance from obstacle on the path [m]
-        min_longitudinal_margin: 2.0         # stop margin distance when any other stop point is inserted in stop margin [m]
-        hold_stop_margin_distance: 0.0       # the ego keeps stopping if the ego is in this margin [m]
+        max_longitudinal_margin: 5.0             # stop margin distance from obstacle on the path [m]
+        max_longitudinal_margin_behind_goal: 3.0 # stop margin distance from obstacle behind the goal on the path [m]
+        min_longitudinal_margin: 2.0             # stop margin distance when any other stop point is inserted in stop margin [m]
+        hold_stop_margin_distance: 0.0           # the ego keeps stopping if the ego is in this margin [m]
 
       # params for detection area
       detection_area:

diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp
@@ -99,6 +99,7 @@ struct StopParam
 
   // margin between obstacle and the ego's front [m]
   double max_longitudinal_margin;
+  double max_longitudinal_margin_behind_goal;
 
   // margin between obstacle and the ego's front [m]
   // if any other stop point is inserted within max_longitudinal_margin.

diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
@@ -85,6 +85,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     // params for stop position
     p.max_longitudinal_margin =
       declare_parameter<double>(ns + "stop_position.max_longitudinal_margin");
+    p.max_longitudinal_margin_behind_goal =
+      declare_parameter<double>(ns + "stop_position.max_longitudinal_margin_behind_goal");
     p.min_longitudinal_margin =
       declare_parameter<double>(ns + "stop_position.min_longitudinal_margin");
     p.hold_stop_margin_distance =
@@ -104,6 +106,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
 
     // apply offset
     p.max_longitudinal_margin += i.max_longitudinal_offset_m;
+    p.max_longitudinal_margin_behind_goal += i.max_longitudinal_offset_m;
     p.min_longitudinal_margin += i.max_longitudinal_offset_m;
     p.stop_search_radius =
       p.step_length +
@@ -1297,8 +1300,13 @@ StopPoint ObstacleStopPlannerNode::searchInsertPoint(
   const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain,
   const StopParam & stop_param)
 {
+  const bool is_behind_goal = static_cast<size_t>(idx) == base_trajectory.size() - 1;
+  const double max_longitudinal_margin = is_behind_goal
+                                           ? stop_param.max_longitudinal_margin_behind_goal
+                                           : stop_param.max_longitudinal_margin;
+
   const auto max_dist_stop_point =
-    createTargetPoint(idx, stop_param.max_longitudinal_margin, base_trajectory, dist_remain);
+    createTargetPoint(idx, max_longitudinal_margin, base_trajectory, dist_remain);
   const auto min_dist_stop_point =
     createTargetPoint(idx, stop_param.min_longitudinal_margin, base_trajectory, dist_remain);