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.

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

fix(mission_planner): revert "refactor(mission_planner): use combineLaneletsShape in lanele2_extension (#5535)" #5543

Merged
merged 1 commit into from
Nov 9, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
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 = lanelet::utils::combineLaneletsShape(lanelets);
lanelet::ConstLanelet combined_lanelets = combine_lanelets(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 = lanelet::utils::combineLaneletsShape(path_lanelets);
lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets);

// check if goal footprint exceeds lane when the goal isn't in parking_lot
if (
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,39 @@
a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
}

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);
}

Check warning on line 88 in planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead

combine_lanelets has 3 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
{
std::vector<geometry_msgs::msg::Point> centerline_points;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ 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