From efff8c356484928fc851e24f81b4a9bedf17be3f Mon Sep 17 00:00:00 2001 From: AhmedEbrahim Date: Fri, 14 Jul 2023 18:55:27 +0300 Subject: [PATCH] fix(obstacle_avoidance_planner): using insertStopPoint instead of current logic Signed-off-by: AhmedEbrahim --- .../obstacle_avoidance_planner/src/node.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 0e34310c32783..71d7dae358539 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -449,24 +449,18 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( if (enable_outside_drivable_area_stop_) { if (first_outside_idx) { - size_t stop_idx = *first_outside_idx; - const auto op_target_point = motion_utils::calcLongitudinalOffsetPoint( - optimized_traj_points, optimized_traj_points.at(stop_idx).pose.position, - -1.0 * vehicle_stop_margin_outside_drivable_area_); - if (op_target_point) { - const auto target_point = op_target_point.get(); - // confirm that target point doesn't overlap with the stop point outside drivable area - const auto dist = - tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(stop_idx), target_point); - const double overlap_threshold = 1e-3; - if (dist > overlap_threshold) { - stop_idx = motion_utils::findNearestSegmentIndex(optimized_traj_points, target_point); + const auto dist = tier4_autoware_utils::calcDistance2d( + optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx)); + + const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; + const auto stop_idx = motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points); + + if (stop_idx) { + publishVirtualWall(optimized_traj_points.at(*stop_idx).pose); + for (size_t i = *stop_idx; i < optimized_traj_points.size(); ++i) { + optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0; } } - publishVirtualWall(optimized_traj_points.at(stop_idx).pose); - for (size_t i = stop_idx; i < optimized_traj_points.size(); ++i) { - optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0; - } } }