Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(behavior_velocity_planner): fix getNearestLaneId() #1564

Closed
wants to merge 2 commits into from
Closed

fix(behavior_velocity_planner): fix getNearestLaneId() #1564

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
04df798
fix(behavior_velocity_planner): fix getNearestLaneId()
rej55 Aug 10, 2022
b26c888
fix(behavior_velocity_planner): fix typo
rej55 Aug 10, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 8 additions & 23 deletions planning/behavior_velocity_planner/src/utilization/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons
for (const auto & p : partition) {
line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()});
}
// corect line to calculate distance accuratry
// correct line to calculate distance in accurate
boost::geometry::correct(line);
polys.emplace_back(lanelet::BasicPolygon2d(line));
}
Expand Down Expand Up @@ -511,36 +511,21 @@ boost::optional<int64_t> getNearestLaneId(
const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
const geometry_msgs::msg::Pose & current_pose, boost::optional<size_t> & nearest_segment_idx)
{
boost::optional<int64_t> nearest_lane_id;

nearest_segment_idx = motion_utils::findNearestSegmentIndex(
path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);

if (!nearest_segment_idx) {
return boost::none;
}

lanelet::ConstLanelets current_lanes;
if (
lanelet::utils::query::getCurrentLanelets(
lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
nearest_segment_idx) {
for (const auto & ll : current_lanes) {
if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) {
nearest_lane_id = ll.id();
return nearest_lane_id;
}
}
lanelet::ConstLanelets lanes;
for (const auto & point : path.points) {
lanes.push_back(lanelet_map->laneletLayer.get(point.lane_ids.at(0)));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

lane_ids.front() do not necessarily contain all lane_ids, so all ids in lane_ids need to be pushed back.

}

// if the lane_id of nearest_segment_idx does not involved in current_lanes,
// search the lane_id of nearest_segment_idx + 1
*nearest_segment_idx += 1;
for (const auto & ll : current_lanes) {
if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) {
nearest_lane_id = ll.id();
return nearest_lane_id;
}
}
lanelet::Lanelet closest_lane;
if (lanelet::utils::query::getClosestLanelet(lanes, current_pose, &closest_lane)) {
return closest_lane.id();
}
return boost::none;
}
Expand Down