From 8ccb11759cac3601cdc0e2cae014d0f0818e9c38 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 3 Jun 2024 02:46:17 +0900 Subject: [PATCH 1/4] feat(route_hanler): handle waypoints in lanelet2 map Signed-off-by: Takayuki Murooka --- .../include/route_handler/route_handler.hpp | 20 ++ planning/route_handler/src/route_handler.cpp | 257 ++++++++++++++++-- 2 files changed, 261 insertions(+), 16 deletions(-) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 24a20f86ed718..5c5adc0e0f7bb 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -51,6 +51,20 @@ enum class Direction { NONE, LEFT, RIGHT }; enum class PullOverDirection { NONE, LEFT, RIGHT }; enum class PullOutDirection { NONE, LEFT, RIGHT }; +struct ReferencePoint +{ + bool is_waypoint{false}; + geometry_msgs::msg::Point point; +}; +using PiecewiseReferencePoints = std::vector; + +struct PiecewiseWaypoints +{ + lanelet::Id lanelet_id; + std::vector piecewise_waypoints; +}; +using Waypoints = std::vector; + class RouteHandler { public: @@ -270,6 +284,12 @@ class RouteHandler PathWithLaneId getCenterLinePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact = true) const; + std::vector calcWaypointsVector(const lanelet::ConstLanelets & lanelet_sequence) const; + 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; 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 d410cb2668175..8f7b02ca056e7 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -38,9 +38,12 @@ #include #include #include +#include #include #include +namespace route_handler +{ namespace { using autoware_planning_msgs::msg::LaneletPrimitive; @@ -129,10 +132,50 @@ std::string toString(const geometry_msgs::msg::Pose & pose) return ss.str(); } -} // namespace +bool isClose( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double epsilon) +{ + return std::abs(p1.x - p2.x) < epsilon && std::abs(p1.y - p2.y) < epsilon; +} -namespace route_handler +PiecewiseReferencePoints convertWaypointsToReferencePoints( + const std::vector & piecewise_waypoints) +{ + PiecewiseReferencePoints piecewise_ref_points; + for (const auto & piecewise_waypoint : piecewise_waypoints) { + piecewise_ref_points.push_back(ReferencePoint{true, piecewise_waypoint}); + } + return piecewise_ref_points; +} + +template +bool isIndexWithinVector(const std::vector & vec, const int index) { + return 0 <= index && index < static_cast(vec.size()); +} + +template +void removeIndicesFromVector(std::vector & vec, const std::vector & indices) +{ + // sort indices in a descending order + auto copied_indices = indices; + std::sort(copied_indices.begin(), copied_indices.end(), std::greater()); + + // remove indices from vector + for (const size_t index : copied_indices) { + vec.erase(vec.begin() + index); + } +} + +lanelet::ArcCoordinates calcArcCoordinates( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & point) +{ + return lanelet::geometry::toArcCoordinates( + to2D(lanelet.centerline()), + to2D(lanelet::utils::conversion::toLaneletPoint(point)).basicPoint()); +} +} // namespace + RouteHandler::RouteHandler(const LaneletMapBin & map_msg) { setMap(map_msg); @@ -1494,21 +1537,21 @@ PathWithLaneId RouteHandler::getCenterLinePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact) const { - PathWithLaneId reference_path{}; - double s = 0; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + // 1. calculate reference points by lanelets' centerlines + // NOTE: This vector alignes 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( + ReferencePoint{false, lanelet::utils::conversion::toGeomMsgPt(pt)}); + }; + double s = 0; for (const auto & llt : lanelet_sequence) { - const lanelet::traffic_rules::SpeedLimitInformation limit = traffic_rules_ptr_->speedLimit(llt); + piecewise_ref_points_vec.push_back(std::vector{}); const lanelet::ConstLineString3d centerline = llt.centerline(); - const auto add_path_point = [&reference_path, &limit, &llt](const auto & pt) { - PathPointWithLaneId p{}; - p.point.pose.position = lanelet::utils::conversion::toGeomMsgPt(pt); - p.lane_ids.push_back(llt.id()); - p.point.longitudinal_velocity_mps = static_cast(limit.speedLimit.value()); - reference_path.points.push_back(p); - }; - for (size_t i = 0; i < centerline.size(); i++) { const auto & pt = centerline[i]; const lanelet::ConstPoint3d next_pt = @@ -1517,19 +1560,85 @@ PathWithLaneId RouteHandler::getCenterLinePath( if (s < s_start && s + distance > s_start) { const auto p = use_exact ? get3DPointFrom2DArcLength(lanelet_sequence, s_start) : pt; - add_path_point(p); + add_ref_point(p); } if (s >= s_start && s <= s_end) { - add_path_point(pt); + add_ref_point(pt); } if (s < s_end && s + distance > s_end) { const auto p = use_exact ? get3DPointFrom2DArcLength(lanelet_sequence, s_end) : next_pt; - add_path_point(p); + add_ref_point(p); } s += distance; } } + // 2. calculate waypoints + const auto waypoints_vec = calcWaypointsVector(lanelet_sequence); + + // 3. remove points in the margin of the waypoint + for (const auto & waypoints : waypoints_vec) { + for (auto piecewise_waypoints_itr = waypoints.begin(); + piecewise_waypoints_itr != waypoints.end(); ++piecewise_waypoints_itr) { + const auto & piecewise_waypoints = piecewise_waypoints_itr->piecewise_waypoints; + const auto lanelet_id = piecewise_waypoints_itr->lanelet_id; + + // calculate index of lanelet_sequence which corresponds to piecewise_waypoints. + const auto lanelet_sequence_itr = std::find_if( + lanelet_sequence.begin(), lanelet_sequence.end(), + [&](const auto & lanelet) { return lanelet.id() == lanelet_id; }); + if (lanelet_sequence_itr == lanelet_sequence.end()) { + continue; + } + const size_t lanelet_sequence_index = + std::distance(lanelet_sequence.begin(), lanelet_sequence_itr); + + // calculate reference points by waypoints + const auto ref_points_by_waypoints = convertWaypointsToReferencePoints(piecewise_waypoints); + + // update reference points by waypoints + if ( + 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; + } 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(); + + // 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); + current_piecewise_ref_points.insert( + is_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 + removeOverlappedCenterlineWithWaypoints( + piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, lanelet_sequence_index, + is_first); + } + } + } + + // 4. convert to PathPointsWithLaneIds + PathWithLaneId reference_path{}; + for (size_t lanelet_idx = 0; lanelet_idx < lanelet_sequence.size(); ++lanelet_idx) { + const auto & lanelet = lanelet_sequence.at(lanelet_idx); + const float speed_limit = + static_cast(traffic_rules_ptr_->speedLimit(lanelet).speedLimit.value()); + + const auto & piecewise_ref_points = piecewise_ref_points_vec.at(lanelet_idx); + for (const auto & ref_point : piecewise_ref_points) { + PathPointWithLaneId p{}; + p.point.pose.position = ref_point.point; + p.lane_ids.push_back(lanelet.id()); + p.point.longitudinal_velocity_mps = speed_limit; + reference_path.points.push_back(p); + } + } reference_path = removeOverlappingPoints(reference_path); // append a point only when having one point so that yaw calculation would work @@ -1565,6 +1674,122 @@ PathWithLaneId RouteHandler::getCenterLinePath( return reference_path; } +std::vector RouteHandler::calcWaypointsVector( + const lanelet::ConstLanelets & lanelet_sequence) const +{ + std::vector waypoints_vec; + for (size_t lanelet_idx = 0; lanelet_idx < lanelet_sequence.size(); ++lanelet_idx) { + const auto & lanelet = lanelet_sequence.at(lanelet_idx); + if (!lanelet.hasAttribute("waypoints")) { + continue; + } + + // generate piecewise waypoints + PiecewiseWaypoints piecewise_waypoints{lanelet.id(), {}}; + const auto waypoints_id = lanelet.attribute("waypoints").asId().value(); + for (const auto & waypoint : lanelet_map_ptr_->lineStringLayer.get(waypoints_id)) { + piecewise_waypoints.piecewise_waypoints.push_back( + lanelet::utils::conversion::toGeomMsgPt(waypoint)); + } + if (piecewise_waypoints.piecewise_waypoints.empty()) { + continue; + } + + // check if the piecewise waypoints are connected to the previous piecewise waypoints + if ( + !waypoints_vec.empty() && isClose( + waypoints_vec.back().back().piecewise_waypoints.back(), + piecewise_waypoints.piecewise_waypoints.front(), 1.0)) { + waypoints_vec.back().push_back(piecewise_waypoints); + } else { + // add new waypoints + Waypoints new_waypoints; + new_waypoints.push_back(piecewise_waypoints); + waypoints_vec.push_back(new_waypoints); + } + } + + return waypoints_vec; +} + +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 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 + + 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; + }(); + + double offset_arc_length = 0.0; + int target_lanelet_sequence_index = static_cast(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( + lanelet_sequence.at(target_lanelet_sequence_index).centerline().basicLineString())); + + // 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) { + const size_t ref_point_index = + is_first ? target_piecewise_ref_points.size() - 1 - ref_point_tmp_index + : ref_point_tmp_index; + const auto & ref_point = target_piecewise_ref_points.at(ref_point_index); + if (ref_point.is_waypoint) { // skip waypoints + continue; + } + + const double ref_point_arc_length = + (is_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) { + return true; + } + } else { + if (front_arc_length_threshold < offset_arc_length + ref_point_arc_length) { + return true; + } + } + + overlapped_ref_points_indices.push_back(ref_point_index); + } + return false; + }(); + + // remove overlapped indices from ref_points + removeIndicesFromVector(target_piecewise_ref_points, overlapped_ref_points_indices); + + // break if searching overlapped centerline is finished. + if (is_search_finished) { + break; + } + + target_lanelet_sequence_index += is_first ? -1 : 1; + offset_arc_length = (is_first ? -1 : 1) * target_lanelet_arc_length; + } +} + bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; From 1cb785a2177f834528a36bd443c7221fac5e9cf5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 4 Jun 2024 01:25:41 +0900 Subject: [PATCH 2/4] 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 5c5adc0e0f7bb..73651fbdcc6a3 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 8f7b02ca056e7..0466401c46bd1 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; } } From 8a4419d46ce9159f5e7f83199ff8d359553e019f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 10 Jun 2024 02:45:48 +0900 Subject: [PATCH 3/4] fix Signed-off-by: Takayuki Murooka --- .../include/route_handler/route_handler.hpp | 3 +- planning/route_handler/src/route_handler.cpp | 80 ++++++++++++------- 2 files changed, 53 insertions(+), 30 deletions(-) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 73651fbdcc6a3..d692337b10bd6 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -289,7 +289,8 @@ class RouteHandler std::vector & piecewise_ref_points_vec, const std::vector & piecewise_waypoints, const lanelet::ConstLanelets & lanelet_sequence, - const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_waypoint_first) const; + const size_t piecewise_waypoints_lanelet_sequence_index, + const bool is_removing_direction_forward) 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 0466401c46bd1..5e90ee18b5fa2 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1596,30 +1596,45 @@ PathWithLaneId RouteHandler::getCenterLinePath( const auto ref_points_by_waypoints = convertWaypointsToReferencePoints(piecewise_waypoints); // update reference points by waypoints - if ( - 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 waypoints. - piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index) = - ref_points_by_waypoints; - } else { + const bool is_first_waypoint_contained = piecewise_waypoints_itr == waypoints.begin(); + const bool is_last_waypoint_contained = piecewise_waypoints_itr == waypoints.end() - 1; + if (is_first_waypoint_contained || is_last_waypoint_contained) { // If piecewise_waypoints_itr is the end (first or last) of piecewise_waypoints - const bool is_waypoint_first = piecewise_waypoints_itr == waypoints.begin(); - // concatenate waypoints and reference points by centerline - // NOTE: They may overlap longitudinally. + const auto original_piecewise_ref_points = + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index); + + // define current_piecewise_ref_points, and initialize it with waypoints auto & current_piecewise_ref_points = piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index); - current_piecewise_ref_points.insert( - is_waypoint_first ? current_piecewise_ref_points.end() - : current_piecewise_ref_points.begin(), - ref_points_by_waypoints.begin(), ref_points_by_waypoints.end()); - - // remove overlapped ref points which overlap waypoints - removeOverlappedCenterlineWithWaypoints( - piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, - piecewise_waypoints_lanelet_sequence_index, is_waypoint_first); + current_piecewise_ref_points = ref_points_by_waypoints; + if (is_first_waypoint_contained) { + // add original reference points to current reference points, and remove reference points + // overlapped with waypoints + current_piecewise_ref_points.insert( + current_piecewise_ref_points.begin(), original_piecewise_ref_points.begin(), + original_piecewise_ref_points.end()); + const bool is_removing_direction_forward = false; + removeOverlappedCenterlineWithWaypoints( + piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, + piecewise_waypoints_lanelet_sequence_index, is_removing_direction_forward); + } + if (is_last_waypoint_contained) { + // add original reference points to current reference points, and remove reference points + // overlapped with waypoints + current_piecewise_ref_points.insert( + current_piecewise_ref_points.end(), original_piecewise_ref_points.begin(), + original_piecewise_ref_points.end()); + const bool is_removing_direction_forward = true; + removeOverlappedCenterlineWithWaypoints( + piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, + piecewise_waypoints_lanelet_sequence_index, is_removing_direction_forward); + } + } else { + // If piecewise_waypoints_itr is not the end (first or last) of piecewise_waypoints, + // remove all the reference points and add waypoints. + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index) = + ref_points_by_waypoints; } } } @@ -1717,7 +1732,8 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints( std::vector & piecewise_ref_points_vec, const std::vector & piecewise_waypoints, const lanelet::ConstLanelets & lanelet_sequence, - const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_waypoint_first) const + const size_t piecewise_waypoints_lanelet_sequence_index, + const bool is_removing_direction_forward) const { const double waypoints_interpolation_arc_margin_ratio = 5.0; @@ -1754,25 +1770,31 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints( ref_point_unsigned_index < target_piecewise_ref_points.size(); ++ref_point_unsigned_index) { const size_t ref_point_index = - is_waypoint_first ? target_piecewise_ref_points.size() - 1 - ref_point_unsigned_index - : ref_point_unsigned_index; + is_removing_direction_forward + ? ref_point_unsigned_index + : target_piecewise_ref_points.size() - 1 - ref_point_unsigned_index; const auto & ref_point = target_piecewise_ref_points.at(ref_point_index); // skip waypoints if (ref_point.is_waypoint) { + if ( + target_lanelet_sequence_index == + static_cast(piecewise_waypoints_lanelet_sequence_index)) { + overlapped_ref_points_indices.clear(); + } continue; } const double ref_point_arc_length = - (is_waypoint_first ? -target_lanelet_arc_length : 0) + + (is_removing_direction_forward ? 0 : -target_lanelet_arc_length) + calcArcCoordinates(lanelet_sequence.at(target_lanelet_sequence_index), ref_point.point) .length; - if (is_waypoint_first) { - if (offset_arc_length + ref_point_arc_length < front_arc_length_threshold) { + if (is_removing_direction_forward) { + if (back_arc_length_threshold < offset_arc_length + ref_point_arc_length) { return true; } } else { - if (back_arc_length_threshold < offset_arc_length + ref_point_arc_length) { + if (offset_arc_length + ref_point_arc_length < front_arc_length_threshold) { return true; } } @@ -1790,8 +1812,8 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints( break; } - target_lanelet_sequence_index += is_waypoint_first ? -1 : 1; - offset_arc_length = (is_waypoint_first ? -1 : 1) * target_lanelet_arc_length; + target_lanelet_sequence_index += is_removing_direction_forward ? 1 : -1; + offset_arc_length = (is_removing_direction_forward ? 1 : -1) * target_lanelet_arc_length; } } From e2dcf3b0b47fa32507924ab7e3354a11104d22f9 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 10 Jun 2024 03:25:32 +0900 Subject: [PATCH 4/4] fix Signed-off-by: Takayuki Murooka --- planning/route_handler/src/route_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 5e90ee18b5fa2..1fd0dd7747e49 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1735,7 +1735,7 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints( const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_removing_direction_forward) const { - const double waypoints_interpolation_arc_margin_ratio = 5.0; + const double waypoints_interpolation_arc_margin_ratio = 10.0; // calculate arc length threshold const double front_arc_length_threshold = [&]() {