From 1c2b489d13aadd0ad7c94337e760dacd69898e8f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 4 Jun 2024 01:25:41 +0900 Subject: [PATCH] fix Signed-off-by: Takayuki Murooka --- .../include/route_handler/route_handler.hpp | 6 +- planning/route_handler/src/route_handler.cpp | 93 ++++++++++--------- 2 files changed, 52 insertions(+), 47 deletions(-) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index bcb5cb6f563cd..0e2cc112c0b5a 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 Tier IV, Inc. +// Copyright 2021-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -288,8 +288,8 @@ class RouteHandler void removeOverlappedCenterlineWithWaypoints( std::vector & piecewise_ref_points_vec, const std::vector & piecewise_waypoints, - const lanelet::ConstLanelets & lanelet_sequence, const size_t lanelet_sequence_index, - const bool is_first) const; + const lanelet::ConstLanelets & lanelet_sequence, + const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_waypoint_first) const; std::optional getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, const Direction direction = Direction::NONE) const; std::optional getLaneChangeTargetExceptPreferredLane( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index e4a5db57db277..6f14cf5bcf107 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 Tier IV, Inc. +// Copyright 2021-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -155,14 +155,13 @@ bool isIndexWithinVector(const std::vector & vec, const int index) } template -void removeIndicesFromVector(std::vector & vec, const std::vector & indices) +void removeIndicesFromVector(std::vector & vec, std::vector indices) { // sort indices in a descending order - auto copied_indices = indices; - std::sort(copied_indices.begin(), copied_indices.end(), std::greater()); + std::sort(indices.begin(), indices.end(), std::greater()); // remove indices from vector - for (const size_t index : copied_indices) { + for (const size_t index : indices) { vec.erase(vec.begin() + index); } } @@ -1540,8 +1539,8 @@ PathWithLaneId RouteHandler::getCenterLinePath( using lanelet::utils::to2D; using lanelet::utils::conversion::toLaneletPoint; - // 1. calculate reference points by lanelets' centerlines - // NOTE: This vector alignes the vector lanelet_sequence. + // 1. calculate reference points by lanelets' centerline + // NOTE: This vector aligns the vector lanelet_sequence. std::vector piecewise_ref_points_vec; const auto add_ref_point = [&](const auto & pt) { piecewise_ref_points_vec.back().push_back( @@ -1550,8 +1549,8 @@ PathWithLaneId RouteHandler::getCenterLinePath( double s = 0; for (const auto & llt : lanelet_sequence) { piecewise_ref_points_vec.push_back(std::vector{}); - const lanelet::ConstLineString3d centerline = llt.centerline(); + const lanelet::ConstLineString3d centerline = llt.centerline(); for (size_t i = 0; i < centerline.size(); i++) { const auto & pt = centerline[i]; const lanelet::ConstPoint3d next_pt = @@ -1590,7 +1589,7 @@ PathWithLaneId RouteHandler::getCenterLinePath( if (lanelet_sequence_itr == lanelet_sequence.end()) { continue; } - const size_t lanelet_sequence_index = + const size_t piecewise_waypoints_lanelet_sequence_index = std::distance(lanelet_sequence.begin(), lanelet_sequence_itr); // calculate reference points by waypoints @@ -1601,24 +1600,26 @@ PathWithLaneId RouteHandler::getCenterLinePath( piecewise_waypoints_itr != waypoints.begin() && piecewise_waypoints_itr != waypoints.end() - 1) { // If piecewise_waypoints_itr is not the end (first or last) of piecewise_waypoints, - // remove all the reference points and add reference points generated by waypoints. - piecewise_ref_points_vec.at(lanelet_sequence_index) = ref_points_by_waypoints; + // remove all the reference points and add waypoints. + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index) = + ref_points_by_waypoints; } else { // If piecewise_waypoints_itr is the end (first or last) of piecewise_waypoints - // TODO(murooka) change the name - const bool is_first = piecewise_waypoints_itr == waypoints.begin(); + const bool is_waypoint_first = piecewise_waypoints_itr == waypoints.begin(); - // concatenate waypoints and ref points - // NOTE: reference points by centerline and waypoints may overlap longitudinally. - auto & current_piecewise_ref_points = piecewise_ref_points_vec.at(lanelet_sequence_index); + // concatenate waypoints and reference points by centerline + // NOTE: They may overlap longitudinally. + auto & current_piecewise_ref_points = + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index); current_piecewise_ref_points.insert( - is_first ? current_piecewise_ref_points.end() : current_piecewise_ref_points.begin(), + is_waypoint_first ? current_piecewise_ref_points.end() + : current_piecewise_ref_points.begin(), ref_points_by_waypoints.begin(), ref_points_by_waypoints.end()); - // remove invalid ref points which overlap waypoints + // remove overlapped ref points which overlap waypoints removeOverlappedCenterlineWithWaypoints( - piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, lanelet_sequence_index, - is_first); + piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, + piecewise_waypoints_lanelet_sequence_index, is_waypoint_first); } } } @@ -1715,31 +1716,32 @@ std::vector RouteHandler::calcWaypointsVector( void RouteHandler::removeOverlappedCenterlineWithWaypoints( std::vector & piecewise_ref_points_vec, const std::vector & piecewise_waypoints, - const lanelet::ConstLanelets & lanelet_sequence, const size_t lanelet_sequence_index, - const bool is_first) const + const lanelet::ConstLanelets & lanelet_sequence, + const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_waypoint_first) const { const double waypoints_interpolation_arc_margin_ratio = 5.0; // calculate arc length threshold const double front_arc_length_threshold = [&]() { - const auto front_waypoint_arc_coordinates = - calcArcCoordinates(lanelet_sequence.at(lanelet_sequence_index), piecewise_waypoints.front()); - return front_waypoint_arc_coordinates.length + + const auto front_waypoint_arc_coordinates = calcArcCoordinates( + lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index), piecewise_waypoints.front()); + const double lanelet_arc_length = boost::geometry::length( + lanelet::utils::to2D(lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index) + .centerline() + .basicLineString())); + return -lanelet_arc_length + front_waypoint_arc_coordinates.length - std::abs(front_waypoint_arc_coordinates.distance) * waypoints_interpolation_arc_margin_ratio; }(); const double back_arc_length_threshold = [&]() { - const auto back_waypoint_arc_coordinates = - calcArcCoordinates(lanelet_sequence.at(lanelet_sequence_index), piecewise_waypoints.back()); - const double lanelet_arc_length = boost::geometry::length(lanelet::utils::to2D( - lanelet_sequence.at(lanelet_sequence_index).centerline().basicLineString())); - return -lanelet_arc_length + back_waypoint_arc_coordinates.length - - std::abs(back_waypoint_arc_coordinates.distance) * - waypoints_interpolation_arc_margin_ratio; + const auto back_waypoint_arc_coordinates = calcArcCoordinates( + lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index), piecewise_waypoints.back()); + return back_waypoint_arc_coordinates.length + std::abs(back_waypoint_arc_coordinates.distance) * + waypoints_interpolation_arc_margin_ratio; }(); double offset_arc_length = 0.0; - int target_lanelet_sequence_index = static_cast(lanelet_sequence_index); + int target_lanelet_sequence_index = static_cast(piecewise_waypoints_lanelet_sequence_index); while (isIndexWithinVector(lanelet_sequence, target_lanelet_sequence_index)) { auto & target_piecewise_ref_points = piecewise_ref_points_vec.at(target_lanelet_sequence_index); const double target_lanelet_arc_length = boost::geometry::length(lanelet::utils::to2D( @@ -1748,26 +1750,29 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints( // search overlapped ref points in the target lanelet std::vector overlapped_ref_points_indices{}; const bool is_search_finished = [&]() { - for (size_t ref_point_tmp_index = 0; ref_point_tmp_index < target_piecewise_ref_points.size(); - ++ref_point_tmp_index) { + for (size_t ref_point_unsigned_index = 0; + ref_point_unsigned_index < target_piecewise_ref_points.size(); + ++ref_point_unsigned_index) { const size_t ref_point_index = - is_first ? target_piecewise_ref_points.size() - 1 - ref_point_tmp_index - : ref_point_tmp_index; + is_waypoint_first ? target_piecewise_ref_points.size() - 1 - ref_point_unsigned_index + : ref_point_unsigned_index; const auto & ref_point = target_piecewise_ref_points.at(ref_point_index); - if (ref_point.is_waypoint) { // skip waypoints + + // skip waypoints + if (ref_point.is_waypoint) { continue; } const double ref_point_arc_length = - (is_first ? -target_lanelet_arc_length : 0) + + (is_waypoint_first ? -target_lanelet_arc_length : 0) + calcArcCoordinates(lanelet_sequence.at(target_lanelet_sequence_index), ref_point.point) .length; - if (is_first) { - if (offset_arc_length + ref_point_arc_length < back_arc_length_threshold) { + if (is_waypoint_first) { + if (offset_arc_length + ref_point_arc_length < front_arc_length_threshold) { return true; } } else { - if (front_arc_length_threshold < offset_arc_length + ref_point_arc_length) { + if (back_arc_length_threshold < offset_arc_length + ref_point_arc_length) { return true; } } @@ -1785,8 +1790,8 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints( break; } - target_lanelet_sequence_index += is_first ? -1 : 1; - offset_arc_length = (is_first ? -1 : 1) * target_lanelet_arc_length; + target_lanelet_sequence_index += is_waypoint_first ? -1 : 1; + offset_arc_length = (is_waypoint_first ? -1 : 1) * target_lanelet_arc_length; } }