fix(slow_down_planner): improper parameter used in slow down (#2276)

* fix(slow_down_planner): improper parameter used in slow down Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_planning_launch): remove hardcoded param enable_slow_down from launch.py Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Nov 29, 2022 · 140c505 · 140c505
1 parent d265c32
commit 140c505
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -209,7 +209,6 @@ def launch_setup(context, *args, **kwargs):
             obstacle_stop_planner_param,
             obstacle_stop_planner_acc_param,
             vehicle_info_param,
-            {"enable_slow_down": False},
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )

diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
@@ -582,13 +582,23 @@ void ObstacleStopPlannerNode::insertVelocity(
       const auto end_insert_point_with_idx = getBackwardInsertPointFromBasePoint(
         index_with_dist_remain.get().first, output, -index_with_dist_remain.get().second);
 
+      const auto slow_down_velocity =
+        slow_down_param_.min_slow_down_velocity +
+        (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
+          std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
+          slow_down_param_.lateral_margin;
+
+      const auto target_velocity = slow_down_param_.consider_constraints
+                                     ? slow_down_param_.slow_down_velocity
+                                     : slow_down_velocity;
+
       SlowDownSection slow_down_section{};
       slow_down_section.slow_down_start_idx = 0;
       slow_down_section.start_point = output.front();
       slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first;
       slow_down_section.end_point = end_insert_point_with_idx.get().second;
-      slow_down_section.velocity = set_velocity_limit_ ? std::numeric_limits<double>::max()
-                                                       : slow_down_param_.slow_down_velocity;
+      slow_down_section.velocity =
+        set_velocity_limit_ ? std::numeric_limits<double>::max() : target_velocity;
 
       insertSlowDownSection(slow_down_section, output);
     } else if (no_hunting_slowdown_point) {