Possible bug of getClosestRouteLaneletFromLanelet() function

[felixf4xu]

In the header of getClosestRouteLaneletFromLanelet function,

autoware_core/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp

Lines 266 to 279 in b2e323a

	/**
	* Finds the closest route lanelet to the search pose satisfying the distance and yaw constraints
	* with respect to a reference lanelet, the search set will include previous route lanelets,
	* next route lanelets, and neighbors of reference lanelet. Returns false if failed to find
	* lanelet.
	* @param search_pose pose to find closest lanelet to
	* @param reference_lanelet reference lanelet to decide the search set
	* @param dist_threshold distance constraint closest lanelet must be within
	* @param yaw_threshold yaw constraint closest lanelet direction must be within
	*/
	bool getClosestRouteLaneletFromLanelet(
	const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet,
	lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
	const double yaw_threshold) const;

it clearly states that

the search set will include previous route lanelets, next route lanelets, and neighbors of reference lanelet

But in the cpp file of getClosestRouteLaneletFromLanelet,

bool RouteHandler::getClosestRouteLaneletFromLanelet(
  const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet,
  lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
  const double yaw_threshold) const
{
  lanelet::ConstLanelets previous_lanelets, next_lanelets, lanelet_sequence;
  if (getPreviousLaneletsWithinRoute(reference_lanelet, &previous_lanelets)) {
    lanelet_sequence = previous_lanelets;
  }

  lanelet_sequence.push_back(reference_lanelet);

  if (getNextLaneletsWithinRoute(reference_lanelet, &next_lanelets)) {
    lanelet_sequence.insert(lanelet_sequence.end(), next_lanelets.begin(), next_lanelets.end());
  }
  auto opt = autoware::experimental::lanelet2_utils::get_closest_lanelet_within_constraint(
    lanelet_sequence, search_pose, dist_threshold, yaw_threshold);
  if (opt.has_value()) {
    *closest_lanelet = *opt;
    return true;
  }

  return false;
}

I see previous_lanelets and next_lanelets, but it seems that some thing like neighbor_lanelets is missing.

I'm not sure if there is a possible bug here.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.