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

refactor(mission_planner): use combineLaneletsShape in lanelet2_extension #5535

Merged
merged 1 commit into from
Nov 9, 2023
+2 −36
Merged

refactor(mission_planner): use combineLaneletsShape in lanelet2_extension #5535

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 2 additions & 2 deletions planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,7 +288,7 @@ bool DefaultPlanner::check_goal_footprint(
lanelet::ConstLanelets lanelets;
lanelets.push_back(combined_prev_lanelet);
lanelets.push_back(next_lane);
lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets);
lanelet::ConstLanelet combined_lanelets = lanelet::utils::combineLaneletsShape(lanelets);

// if next lanelet length longer than vehicle longitudinal offset
if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
Expand Down Expand Up @@ -347,7 +347,7 @@ bool DefaultPlanner::is_goal_valid(

double next_lane_length = 0.0;
// combine calculated route lanelets
lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets);
lanelet::ConstLanelet combined_prev_lanelet = lanelet::utils::combineLaneletsShape(path_lanelets);

// check if goal footprint exceeds lane when the goal isn't in parking_lot
if (
Expand Down
33 changes: 0 additions & 33 deletions planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,40 +53,7 @@
{
a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
}

Check notice on line 56 in planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

combine_lanelets is no longer above the threshold for logical blocks with deeply nested code
lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets)
{
lanelet::Points3d lefts;
lanelet::Points3d rights;
lanelet::Points3d centers;
std::vector<uint64_t> left_bound_ids;
std::vector<uint64_t> right_bound_ids;

for (const auto & llt : lanelets) {
if (llt.id() != 0) {
left_bound_ids.push_back(llt.leftBound().id());
right_bound_ids.push_back(llt.rightBound().id());
}
}

for (const auto & llt : lanelets) {
if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
for (const auto & pt : llt.leftBound()) {
lefts.push_back(lanelet::Point3d(pt));
}
}
if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
for (const auto & pt : llt.rightBound()) {
rights.push_back(lanelet::Point3d(pt));
}
}
}
const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
return std::move(combined_lanelet);
}

std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
{
std::vector<geometry_msgs::msg::Point> centerline_points;
Expand Down
1 change: 0 additions & 1 deletion planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub
void insert_marker_array(
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);

lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets);
std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
geometry_msgs::msg::Pose convertBasicPoint3dToPose(
const lanelet::BasicPoint3d & point, const double lane_yaw);
Expand Down
Loading