fix(behavior_path_planner): add angle limits to nearest point search (t…

…ier4#595)

* fix(behavior_path_planner): add angle limits to nearest point search

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp

@@ -59,11 +59,10 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv
 
 Path toPath(const PathWithLaneId & input);
 
-size_t getIdxByArclength(
-  const PathWithLaneId & path, const Point & origin, const double signed_arc);
+size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const double signed_arc);
 
 void clipPathLength(
-  PathWithLaneId & path, const Point base_pos, const double forward, const double backward);
+  PathWithLaneId & path, const Pose base_pose, const double forward, const double backward);
 
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -686,11 +686,11 @@ void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
 
 void BehaviorPathPlannerNode::clipPathLength(PathWithLaneId & path) const
 {
-  const auto ego_pos = planner_data_->self_pose->pose.position;
+  const auto ego_pose = planner_data_->self_pose->pose;
   const double forward = planner_data_->parameters.forward_path_length;
   const double backward = planner_data_->parameters.backward_path_length;
 
-  util::clipPathLength(path, ego_pos, forward, backward);
+  util::clipPathLength(path, ego_pose, forward, backward);
 }
 
 PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(

planning/behavior_path_planner/src/path_utilities.cpp

@@ -177,13 +177,19 @@ Path toPath(const PathWithLaneId & input)
   return output;
 }
 
-size_t getIdxByArclength(const PathWithLaneId & path, const Point & origin, const double signed_arc)
+size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const double signed_arc)
 {
   if (path.points.empty()) {
     throw std::runtime_error("[getIdxByArclength] path points must be > 0");
   }
 
-  const auto closest_idx = tier4_autoware_utils::findNearestIndex(path.points, origin);
+  const auto boost_closest_idx = tier4_autoware_utils::findNearestIndex(
+    path.points, origin, std::numeric_limits<double>::max(), M_PI / 4.0);
+
+  // If the nearest index search with angle limit fails, search again without angle limit.
+  size_t closest_idx = boost_closest_idx
+                         ? *boost_closest_idx
+                         : tier4_autoware_utils::findNearestIndex(path.points, origin.position);
 
   using tier4_autoware_utils::calcDistance2d;
   double sum_length = 0.0;
@@ -209,14 +215,14 @@ size_t getIdxByArclength(const PathWithLaneId & path, const Point & origin, cons
 }
 
 void clipPathLength(
-  PathWithLaneId & path, const Point base_pos, const double forward, const double backward)
+  PathWithLaneId & path, const Pose base_pose, const double forward, const double backward)
 {
   if (path.points.size() < 3) {
     return;
   }
 
-  const auto start_idx = util::getIdxByArclength(path, base_pos, -backward);
-  const auto end_idx = util::getIdxByArclength(path, base_pos, forward);
+  const auto start_idx = util::getIdxByArclength(path, base_pose, -backward);
+  const auto end_idx = util::getIdxByArclength(path, base_pose, forward);
 
   const std::vector<PathPointWithLaneId> clipped_points{
     path.points.begin() + start_idx, path.points.begin() + end_idx + 1};

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2520,7 +2520,7 @@ void AvoidanceModule::clipPathLength(PathWithLaneId & path) const
   const double forward = planner_data_->parameters.forward_path_length;
   const double backward = planner_data_->parameters.backward_path_length;
 
-  util::clipPathLength(path, getEgoPosition(), forward, backward);
+  util::clipPathLength(path, getEgoPose().pose, forward, backward);
 }
 
 bool AvoidanceModule::isTargetObjectType(const PredictedObject & object) const

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -338,7 +338,7 @@ ShiftPoint SideShiftModule::calcShiftPoint() const
 {
   const auto & p = parameters_;
   const auto ego_speed = std::abs(planner_data_->self_odometry->twist.twist.linear.x);
-  const auto ego_point = planner_data_->self_pose->pose.position;
+  const auto ego_pose = planner_data_->self_pose->pose;
 
   const double dist_to_start =
     std::max(p.min_distance_to_start_shifting, ego_speed * p.time_to_start_shifting);
@@ -357,9 +357,9 @@ ShiftPoint SideShiftModule::calcShiftPoint() const
 
   ShiftPoint shift_point;
   shift_point.length = lateral_offset_;
-  shift_point.start_idx = util::getIdxByArclength(*reference_path_, ego_point, dist_to_start);
+  shift_point.start_idx = util::getIdxByArclength(*reference_path_, ego_pose, dist_to_start);
   shift_point.start = reference_path_->points.at(shift_point.start_idx).point.pose;
-  shift_point.end_idx = util::getIdxByArclength(*reference_path_, ego_point, dist_to_end);
+  shift_point.end_idx = util::getIdxByArclength(*reference_path_, ego_pose, dist_to_end);
   shift_point.end = reference_path_->points.at(shift_point.end_idx).point.pose;
 
   return shift_point;