diff --git a/planning/autoware_route_handler/CMakeLists.txt b/planning/autoware_route_handler/CMakeLists.txt new file mode 100644 index 0000000000..ad94e57088 --- /dev/null +++ b/planning/autoware_route_handler/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_route_handler) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/route_handler.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_autoware_route_handler + test/test_route_handler.cpp) + + ament_target_dependencies(test_autoware_route_handler + autoware_test_utils + ) + + target_link_libraries(test_autoware_route_handler + ${PROJECT_NAME} + ) + +endif() + +ament_auto_package(INSTALL_TO_SHARE + test_route +) diff --git a/planning/autoware_route_handler/README.md b/planning/autoware_route_handler/README.md new file mode 100644 index 0000000000..bc62f06db7 --- /dev/null +++ b/planning/autoware_route_handler/README.md @@ -0,0 +1,18 @@ +# route handler + +`route_handler` is a library for calculating driving route on the lanelet map. + +## Unit Testing + +The unit testing depends on `autoware_test_utils` package. +`autoware_test_utils` is a library that provides several common functions to simplify unit test creating. + +![route_handler_test](./images/route_handler_test.svg) + +By default, route file is necessary to create tests. The following illustrates the route that are used in the unit test + +### Lane change test route + +![lane_change_test_route](./images/lane_change_test_route.svg) + +- The route is based on map that can be obtained from `autoware_test_utils\test_map` diff --git a/planning/autoware_route_handler/images/lane_change_test_route.svg b/planning/autoware_route_handler/images/lane_change_test_route.svg new file mode 100644 index 0000000000..e71c8e13b1 --- /dev/null +++ b/planning/autoware_route_handler/images/lane_change_test_route.svg @@ -0,0 +1,618 @@ + + + + + + + + + + + + + +
+
+
+ 4765 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ 4770 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4775 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4424 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4780 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5088 +
+
+
+ + + + + + + + + + + + +
+
+
+
goal pose
+ (70.0, -1.75) +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ 9590 +
+
+
+ + + + + + + + + + + + +
+
+
+ 9594 +
+
+
+ + + + + + + + + + + + +
+
+
+ 9598 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5072 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5084 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4765 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + +
+
+
+ preferred primitive +
+
+
+ + + + + + + + + +
+
+
+ primitive +
+
+
+ + + + + + + + + + + + +
+
+
+ pose1 +
+ (0.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ check point +
+ (25.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+ start pose +
+ (-50.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_route_handler/images/route_handler_test.svg b/planning/autoware_route_handler/images/route_handler_test.svg new file mode 100644 index 0000000000..445fec1dd8 --- /dev/null +++ b/planning/autoware_route_handler/images/route_handler_test.svg @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + +
+
+
+ + route msg in yaml format +
+ + (to simulate /planning/mission_planner/route topic) +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ mock_data_parser +
+
+
+ + + + + + + + + + + + +
+
+
+ LaneletRoute +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ lanelet file +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ HADMapBin +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ test route handler +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ autoware_test_utils +
+
+
+ + + + + + + diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp new file mode 100644 index 0000000000..9c48083d42 --- /dev/null +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -0,0 +1,431 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__ROUTE_HANDLER__ROUTE_HANDLER_HPP_ +#define AUTOWARE__ROUTE_HANDLER__ROUTE_HANDLER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::route_handler +{ +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using std_msgs::msg::Header; +using unique_identifier_msgs::msg::UUID; +using RouteSections = std::vector; + +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: + RouteHandler() = default; + explicit RouteHandler(const LaneletMapBin & map_msg); + + // non-const methods + void setMap(const LaneletMapBin & map_msg); + void setRoute(const LaneletRoute & route_msg); + void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); + void clearRoute(); + + // const methods + + // for route handler status + bool isHandlerReady() const; + lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const; + Header getRouteHeader() const; + UUID getRouteUuid() const; + bool isAllowedGoalModification() const; + + // for routing graph + bool isMapMsgReady() const; + lanelet::routing::RoutingGraphPtr getRoutingGraphPtr() const; + lanelet::traffic_rules::TrafficRulesPtr getTrafficRulesPtr() const; + std::shared_ptr getOverallGraphPtr() const; + lanelet::LaneletMapPtr getLaneletMapPtr() const; + static bool isNoDrivableLane(const lanelet::ConstLanelet & llt); + + // for routing + bool planPathLaneletsBetweenCheckpoints( + const Pose & start_checkpoint, const Pose & goal_checkpoint, + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const; + std::vector createMapSegments(const lanelet::ConstLanelets & path_lanelets) const; + static bool isRouteLooped(const RouteSections & route_sections); + + // for goal + bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; + Pose getGoalPose() const; + Pose getStartPose() const; + Pose getOriginalStartPose() const; + Pose getOriginalGoalPose() const; + lanelet::Id getGoalLaneId() const; + bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; + std::vector getLanesAfterGoal(const double vehicle_length) const; + + // for lanelet + bool getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; + bool getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const; + bool getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; + bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const; + + /** + * @brief Check if same-direction lane is available at the right side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + std::optional getRightLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = true) const; + + /** + * @brief Check if same-direction lane is available at the left side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + std::optional getLeftLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = true) const; + lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; + + /** + * @brief Check if opposite-direction lane is available at the right side of the lanelet + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet with opposite direction if true + */ + lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet) const; + + /** + * @brief Check if opposite-direction lane is available at the left side of the lanelet + * Required the linestring to be shared between(same line ID) the lanelets. + * @param the lanelet of interest + * @return vector of lanelet with opposite direction if true + */ + lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const; + + /** + * @brief Searches and return all lanelet on the left that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to include the lane with opposite direction + * @param (optional) flag to invert the opposite lanelet + * @return vector of lanelet that is connected via share linestring + */ + lanelet::ConstLanelets getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite, + const bool & invert_opposite = false) const noexcept; + + /** + * @brief Searches and return all lanelet on the right that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to include the lane with opposite direction + * @param (optional) flag to invert the opposite lanelet + * @return vector of lanelet that is connected via share linestring + */ + lanelet::ConstLanelets getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite, + const bool & invert_opposite = false) const noexcept; + + /** + * @brief Searches and return all lanelet (left and right) that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to search only right side + * @param (optional) flag to search only left side + * @param (optional) flag to include the lane with opposite direction + * @param (optional) flag to invert the opposite lanelet + * @return vector of lanelet that is connected via share linestring + */ + lanelet::ConstLanelets getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, + bool is_opposite = true, const bool & invert_opposite = false) const noexcept; + + /** + * @brief Check if same-direction lane is available at the right side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + lanelet::ConstLanelet getMostRightLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; + + /** + * @brief Check if same-direction lane is available at the left side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + lanelet::ConstLanelet getMostLeftLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; + + /** + * Retrieves a sequence of lanelets before the given lanelet. + * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence + * does not include input lanelet.] + * @param graph [input lanelet routing graph] + * @param lanelet [input lanelet] + * @param length [minimum length of retrieved lanelet sequence] + * @return [lanelet sequence that leads to given lanelet] + */ + std::vector getPrecedingLaneletSequence( + const lanelet::ConstLanelet & lanelet, const double length, + const lanelet::ConstLanelets & exclude_lanelets = {}) const; + + /** + * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return + * the number of lane-changeable lane to the preferred lane. + * @param Desired lanelet to query + * @param lane change direction + * @return number of lanes from input to the preferred lane + */ + int getNumLaneToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + + /** + * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return + * the distance to the preferred lane from the give lane. + * This computes each lateral interval to the preferred lane from the given lanelet + * @param lanelet lanelet to query + * @param direction change direction + * @return number of lanes from input to the preferred lane + */ + std::vector getLateralIntervalsToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + + bool getClosestLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestLaneletWithConstrainsWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, + const double yaw_threshold) const; + + /** + * Finds the closest route lanelet to the search pose satisfying the distance and yaw constraints + * with respect to a reference lanelet, the search set will include previous route lanelets, + * next route lanelets, and neighbors of reference lanelet. Returns false if failed to find + * lanelet. + * @param search_pose pose to find closest lanelet to + * @param reference_lanelet reference lanelet to decide the search set + * @param dist_threshold distance constraint closest lanelet must be within + * @param yaw_threshold yaw constraint closest lanelet direction must be within + */ + bool getClosestRouteLaneletFromLanelet( + const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet, + lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, + const double yaw_threshold) const; + + lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const; + lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const; + lanelet::ConstLanelets getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const Pose & current_pose, + const double backward_distance, const double forward_distance, + const bool only_route_lanes = true) const; + lanelet::ConstLanelets getLaneletSequence( + const lanelet::ConstLanelet & lanelet, + const double backward_distance = std::numeric_limits::max(), + const double forward_distance = std::numeric_limits::max(), + const bool only_route_lanes = true) const; + lanelet::ConstLanelets getShoulderLaneletSequence( + const lanelet::ConstLanelet & lanelet, const Pose & pose, + const double backward_distance = std::numeric_limits::max(), + const double forward_distance = std::numeric_limits::max()) const; + bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; + bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const; + bool isRoadLanelet(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getPreferredLanelets() const; + + // for path + 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 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( + const lanelet::ConstLanelets & lanelets, const Direction direction) const; + std::optional getPullOverTarget(const Pose & goal_pose) const; + std::optional getPullOutStartLane( + const Pose & pose, const double vehicle_width) const; + lanelet::ConstLanelets getRoadLaneletsAtPose(const Pose & pose) const; + std::optional getLeftShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const; + std::optional getRightShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const; + + /** + * @brief Search and return shoulder lanelets that intersect with a given pose. + * @param pose reference pose at which to search for shoulder lanelets. + * @return vector of shoulder lanelets intersecting with given pose. + */ + lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; + + Pose get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const; + +private: + // MUST + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + std::shared_ptr overall_graphs_ptr_; + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::ConstLanelets route_lanelets_; + lanelet::ConstLanelets preferred_lanelets_; + lanelet::ConstLanelets start_lanelets_; + lanelet::ConstLanelets goal_lanelets_; + std::shared_ptr route_ptr_{nullptr}; + + rclcpp::Logger logger_{rclcpp::get_logger("route_handler")}; + + bool is_map_msg_ready_{false}; + bool is_handler_ready_{false}; + + // save original(not modified) route start pose for start planer execution + Pose original_start_pose_; + Pose original_goal_pose_; + + // non-const methods + void setLaneletsFromRouteMsg(); + + // const methods + // for routing + lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const; + + // for lanelet + lanelet::ConstLanelets getRouteLanelets() const; + lanelet::ConstLanelets getLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, + const double min_length = std::numeric_limits::max(), + const bool only_route_lanes = true) const; + lanelet::ConstLanelets getLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, + const double min_length = std::numeric_limits::max(), + const bool only_route_lanes = true) const; + std::optional getFollowingShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getShoulderLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, + const double min_length = std::numeric_limits::max()) const; + std::optional getPreviousShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getShoulderLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, + const double min_length = std::numeric_limits::max()) const; + lanelet::ConstLanelets getPreviousLaneletSequence( + const lanelet::ConstLanelets & lanelet_sequence) const; + lanelet::ConstLanelets getNeighborsWithinRoute(const lanelet::ConstLanelet & lanelet) const; + + // for path + + /** + * @brief Checks if a path has a no_drivable_lane or not + * @param path lanelet path + * @return true if the lanelet path includes at least one no_drivable_lane, false if it does not + * include any. + */ + bool hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & path) const; + /** + * @brief Searches for the shortest path between start and goal lanelets that does not include any + * no_drivable_lane. + * @param start_lanelet start lanelet + * @param goal_lanelet goal lanelet + * @return the lanelet path (if found) + */ + std::optional findDrivableLanePath( + const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet) const; +}; + +/// @brief custom routing cost with infinity cost for no drivable lanes +class RoutingCostDrivable : public lanelet::routing::RoutingCostDistance +{ +public: + RoutingCostDrivable() : lanelet::routing::RoutingCostDistance(10.0) {} + inline double getCostSucceeding( + const lanelet::traffic_rules::TrafficRules & trafficRules, + const lanelet::ConstLaneletOrArea & from, const lanelet::ConstLaneletOrArea & to) const + { + if ( + (from.isLanelet() && RouteHandler::isNoDrivableLane(*from.lanelet())) || + (to.isLanelet() && RouteHandler::isNoDrivableLane(*to.lanelet()))) + return std::numeric_limits::infinity(); + return lanelet::routing::RoutingCostDistance::getCostSucceeding(trafficRules, from, to); + } + inline double getCostLaneChange( + const lanelet::traffic_rules::TrafficRules & trafficRules, const lanelet::ConstLanelets & from, + const lanelet::ConstLanelets & to) const noexcept + { + if ( + std::any_of(from.begin(), from.end(), RouteHandler::isNoDrivableLane) || + std::any_of(to.begin(), to.end(), RouteHandler::isNoDrivableLane)) + return std::numeric_limits::infinity(); + return lanelet::routing::RoutingCostDistance::getCostLaneChange(trafficRules, from, to); + } +}; // class RoutingCostDrivable +} // namespace autoware::route_handler +#endif // AUTOWARE__ROUTE_HANDLER__ROUTE_HANDLER_HPP_ diff --git a/planning/autoware_route_handler/package.xml b/planning/autoware_route_handler/package.xml new file mode 100644 index 0000000000..6db78908be --- /dev/null +++ b/planning/autoware_route_handler/package.xml @@ -0,0 +1,40 @@ + + + + autoware_route_handler + 0.0.0 + The route_handling package + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Takayuki Murooka + Mamoru Sobue + Go Sakayori + Alqudah Mohammad + + Apache License 2.0 + + Fumiya Watanabe + + ament_cmake_auto + autoware_cmake + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils + + autoware_lanelet2_extension + autoware_map_msgs + autoware_planning_msgs + autoware_utils + geometry_msgs + rclcpp + rclcpp_components + tf2_ros + yaml-cpp + + + ament_cmake + + diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp new file mode 100644 index 0000000000..5bb5e2a220 --- /dev/null +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -0,0 +1,2114 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/route_handler/route_handler.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::route_handler +{ +namespace +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::Path; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_yaw; +using geometry_msgs::msg::Pose; +using lanelet::utils::to2D; + +bool exists(const std::vector & primitives, const int64_t & id) +{ + for (const auto & p : primitives) { + if (p.id == id) { + return true; + } + } + return false; +} + +bool exists(const lanelet::ConstLanelets & vectors, const lanelet::ConstLanelet & item) +{ + for (const auto & i : vectors) { + if (i.id() == item.id()) { + return true; + } + } + return false; +} + +lanelet::ConstPoint3d get3DPointFrom2DArcLength( + const lanelet::ConstLanelets & lanelet_sequence, const double s) +{ + double accumulated_distance2d = 0; + for (const auto & llt : lanelet_sequence) { + const auto & centerline = llt.centerline(); + lanelet::ConstPoint3d prev_pt; + if (!centerline.empty()) { + prev_pt = centerline.front(); + } + for (const auto & pt : centerline) { + const double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + if (accumulated_distance2d + distance2d > s) { + const double ratio = (s - accumulated_distance2d) / distance2d; + const auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; + return lanelet::ConstPoint3d{ + lanelet::InvalId, interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()}; + } + accumulated_distance2d += distance2d; + prev_pt = pt; + } + } + return lanelet::ConstPoint3d{}; +} + +PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) +{ + PathWithLaneId filtered_path; + for (const auto & pt : input_path.points) { + if (filtered_path.points.empty()) { + filtered_path.points.push_back(pt); + continue; + } + + constexpr double min_dist = 0.001; + if (autoware_utils::calc_distance3d(filtered_path.points.back().point, pt.point) < min_dist) { + filtered_path.points.back().lane_ids.push_back(pt.lane_ids.front()); + filtered_path.points.back().point.longitudinal_velocity_mps = std::min( + pt.point.longitudinal_velocity_mps, + filtered_path.points.back().point.longitudinal_velocity_mps); + } else { + filtered_path.points.push_back(pt); + } + } + filtered_path.left_bound = input_path.left_bound; + filtered_path.right_bound = input_path.right_bound; + return filtered_path; +} + +std::string toString(const geometry_msgs::msg::Pose & pose) +{ + std::stringstream ss; + ss << "(" << pose.position.x << ", " << pose.position.y << "," << pose.position.z << ")"; + return ss.str(); +} + +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; +} + +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, std::vector indices) +{ + // sort indices in a descending order + std::sort(indices.begin(), indices.end(), std::greater()); + + // remove indices from vector + for (const size_t index : 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); + route_ptr_ = nullptr; +} + +void RouteHandler::setMap(const LaneletMapBin & map_msg) +{ + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + const auto map_major_version_opt = + lanelet::io_handlers::parseMajorVersion(map_msg.version_map_format); + if (!map_major_version_opt) { + RCLCPP_WARN( + logger_, "setMap() for invalid version map: %s", map_msg.version_map_format.c_str()); + } else if (map_major_version_opt.value() > static_cast(lanelet::autoware::version)) { + RCLCPP_WARN( + logger_, "setMap() for a map(version %s) newer than lanelet2_extension support version(%d)", + map_msg.version_map_format.c_str(), static_cast(lanelet::autoware::version)); + } + + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + const lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + const lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + const lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + + is_map_msg_ready_ = true; + is_handler_ready_ = false; + + setLaneletsFromRouteMsg(); +} + +bool RouteHandler::isRouteLooped(const RouteSections & route_sections) +{ + std::set lane_primitives; + for (const auto & route_section : route_sections) { + for (const auto & primitive : route_section.primitives) { + if (lane_primitives.find(primitive.id) != lane_primitives.end()) { + return true; // find duplicated id + } + lane_primitives.emplace(primitive.id); + } + } + return false; +} + +void RouteHandler::setRoute(const LaneletRoute & route_msg) +{ + if (!isRouteLooped(route_msg.segments)) { + // if get not modified route but new route, reset original start pose + if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { + original_start_pose_ = route_msg.start_pose; + original_goal_pose_ = route_msg.goal_pose; + } + route_ptr_ = std::make_shared(route_msg); + is_handler_ready_ = false; + setLaneletsFromRouteMsg(); + } else { + RCLCPP_ERROR( + logger_, + "Loop detected within route! Currently, no loop is allowed for route! Using previous route"); + } +} + +bool RouteHandler::isHandlerReady() const +{ + return is_handler_ready_; +} + +void RouteHandler::setRouteLanelets(const lanelet::ConstLanelets & path_lanelets) +{ + if (!path_lanelets.empty()) { + const auto & first_lanelet = path_lanelets.front(); + start_lanelets_ = lanelet::utils::query::getAllNeighbors(routing_graph_ptr_, first_lanelet); + const auto & last_lanelet = path_lanelets.back(); + goal_lanelets_ = lanelet::utils::query::getAllNeighbors(routing_graph_ptr_, last_lanelet); + } + + // set route lanelets + std::unordered_set route_lanelets_id; + std::unordered_set candidate_lanes_id; + for (const auto & lane : path_lanelets) { + route_lanelets_id.insert(lane.id()); + const auto right_relations = routing_graph_ptr_->rightRelations(lane); + for (const auto & right_relation : right_relations) { + if (right_relation.relationType == lanelet::routing::RelationType::Right) { + route_lanelets_id.insert(right_relation.lanelet.id()); + } else if (right_relation.relationType == lanelet::routing::RelationType::AdjacentRight) { + candidate_lanes_id.insert(right_relation.lanelet.id()); + } + } + const auto left_relations = routing_graph_ptr_->leftRelations(lane); + for (const auto & left_relation : left_relations) { + if (left_relation.relationType == lanelet::routing::RelationType::Left) { + route_lanelets_id.insert(left_relation.lanelet.id()); + } else if (left_relation.relationType == lanelet::routing::RelationType::AdjacentLeft) { + candidate_lanes_id.insert(left_relation.lanelet.id()); + } + } + } + + // check if candidates are really part of route + for (const auto & candidate_id : candidate_lanes_id) { + lanelet::ConstLanelet lanelet = lanelet_map_ptr_->laneletLayer.get(candidate_id); + auto previous_lanelets = routing_graph_ptr_->previous(lanelet); + bool is_connected_to_main_lanes_prev = false; + bool is_connected_to_candidate_prev = true; + if (exists(start_lanelets_, lanelet)) { + is_connected_to_candidate_prev = false; + } + while (!previous_lanelets.empty() && is_connected_to_candidate_prev && + !is_connected_to_main_lanes_prev) { + is_connected_to_candidate_prev = false; + + for (const auto & prev_lanelet : previous_lanelets) { + if (route_lanelets_id.find(prev_lanelet.id()) != route_lanelets_id.end()) { + is_connected_to_main_lanes_prev = true; + break; + } + if (exists(start_lanelets_, prev_lanelet)) { + break; + } + + if (candidate_lanes_id.find(prev_lanelet.id()) != candidate_lanes_id.end()) { + is_connected_to_candidate_prev = true; + previous_lanelets = routing_graph_ptr_->previous(prev_lanelet); + break; + } + } + } + + auto following_lanelets = routing_graph_ptr_->following(lanelet); + bool is_connected_to_main_lanes_next = false; + bool is_connected_to_candidate_next = true; + if (exists(goal_lanelets_, lanelet)) { + is_connected_to_candidate_next = false; + } + while (!following_lanelets.empty() && is_connected_to_candidate_next && + !is_connected_to_main_lanes_next) { + is_connected_to_candidate_next = false; + for (const auto & next_lanelet : following_lanelets) { + if (route_lanelets_id.find(next_lanelet.id()) != route_lanelets_id.end()) { + is_connected_to_main_lanes_next = true; + break; + } + if (exists(goal_lanelets_, next_lanelet)) { + break; + } + if (candidate_lanes_id.find(next_lanelet.id()) != candidate_lanes_id.end()) { + is_connected_to_candidate_next = true; + following_lanelets = routing_graph_ptr_->following(next_lanelet); + break; + } + } + } + + if (is_connected_to_main_lanes_next && is_connected_to_main_lanes_prev) { + route_lanelets_id.insert(candidate_id); + } + } + + route_lanelets_.clear(); + route_lanelets_.reserve(route_lanelets_id.size()); + for (const auto & id : route_lanelets_id) { + route_lanelets_.push_back(lanelet_map_ptr_->laneletLayer.get(id)); + } + is_handler_ready_ = true; +} + +void RouteHandler::clearRoute() +{ + route_lanelets_.clear(); + preferred_lanelets_.clear(); + start_lanelets_.clear(); + goal_lanelets_.clear(); + route_ptr_ = nullptr; + is_handler_ready_ = false; +} + +void RouteHandler::setLaneletsFromRouteMsg() +{ + if (!route_ptr_ || !is_map_msg_ready_) { + return; + } + route_lanelets_.clear(); + preferred_lanelets_.clear(); + const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_); + if (!is_route_valid) { + return; + } + + size_t primitive_size{0}; + for (const auto & route_section : route_ptr_->segments) { + primitive_size += route_section.primitives.size(); + } + route_lanelets_.reserve(primitive_size); + + for (const auto & route_section : route_ptr_->segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); + route_lanelets_.push_back(llt); + if (id == route_section.preferred_primitive.id) { + preferred_lanelets_.push_back(llt); + } + } + } + goal_lanelets_.clear(); + start_lanelets_.clear(); + if (!route_ptr_->segments.empty()) { + goal_lanelets_.reserve(route_ptr_->segments.back().primitives.size()); + for (const auto & primitive : route_ptr_->segments.back().primitives) { + const auto id = primitive.id; + const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); + goal_lanelets_.push_back(llt); + } + start_lanelets_.reserve(route_ptr_->segments.front().primitives.size()); + for (const auto & primitive : route_ptr_->segments.front().primitives) { + const auto id = primitive.id; + const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); + start_lanelets_.push_back(llt); + } + } + is_handler_ready_ = true; +} + +Header RouteHandler::getRouteHeader() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getRouteHeader: Route has not been set yet"); + return Header(); + } + return route_ptr_->header; +} + +UUID RouteHandler::getRouteUuid() const +{ + if (!route_ptr_) { + RCLCPP_WARN_SKIPFIRST(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); + return UUID(); + } + return route_ptr_->uuid; +} + +bool RouteHandler::isAllowedGoalModification() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); + return false; + } + return route_ptr_->allow_modification; +} + +std::vector RouteHandler::getLanesAfterGoal( + const double vehicle_length) const +{ + lanelet::ConstLanelet goal_lanelet; + if (!getGoalLanelet(&goal_lanelet)) { + return std::vector{}; + } + + const double min_succeeding_length = vehicle_length * 2; + const auto succeeding_lanes_vec = lanelet::utils::query::getSucceedingLaneletSequences( + routing_graph_ptr_, goal_lanelet, min_succeeding_length); + if (succeeding_lanes_vec.empty()) { + return std::vector{}; + } + + return succeeding_lanes_vec.front(); +} + +lanelet::ConstLanelets RouteHandler::getRouteLanelets() const +{ + return route_lanelets_; +} + +lanelet::ConstLanelets RouteHandler::getPreferredLanelets() const +{ + return preferred_lanelets_; +} + +Pose RouteHandler::getStartPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); + return Pose(); + } + return route_ptr_->start_pose; +} + +Pose RouteHandler::getOriginalStartPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet"); + return Pose(); + } + return original_start_pose_; +} + +Pose RouteHandler::getGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); + return Pose(); + } + return route_ptr_->goal_pose; +} + +Pose RouteHandler::getOriginalGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet"); + return Pose(); + } + return original_goal_pose_; +} + +lanelet::Id RouteHandler::getGoalLaneId() const +{ + if (!route_ptr_ || route_ptr_->segments.empty()) { + return lanelet::InvalId; + } + + return route_ptr_->segments.back().preferred_primitive.id; +} + +bool RouteHandler::getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const +{ + const lanelet::Id goal_lane_id = getGoalLaneId(); + for (const auto & llt : route_lanelets_) { + if (llt.id() == goal_lane_id) { + *goal_lanelet = llt; + return true; + } + } + return false; +} + +bool RouteHandler::isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const +{ + if (!route_ptr_ || route_ptr_->segments.empty()) { + return false; + } + return exists(route_ptr_->segments.back().primitives, lanelet.id()); +} + +lanelet::ConstLanelets RouteHandler::getLaneletsFromIds(const lanelet::Ids & ids) const +{ + lanelet::ConstLanelets lanelets; + lanelets.reserve(ids.size()); + for (const auto & id : ids) { + lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(id)); + } + return lanelets; +} + +lanelet::ConstLanelet RouteHandler::getLaneletsFromId(const lanelet::Id id) const +{ + return lanelet_map_ptr_->laneletLayer.get(id); +} + +bool RouteHandler::isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const +{ + lanelet::ConstLanelet next_lanelet; + return !getNextLaneletWithinRoute(lanelet, &next_lanelet); +} + +lanelet::ConstLanelets RouteHandler::getLaneChangeableNeighbors( + const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::query::getLaneChangeableNeighbors(routing_graph_ptr_, lanelet); +} + +lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, const double min_length, const bool only_route_lanes) const +{ + lanelet::ConstLanelets lanelet_sequence_forward; + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { + return lanelet_sequence_forward; + } + + double length = 0; + lanelet::ConstLanelet current_lanelet = lanelet; + while (rclcpp::ok() && length < min_length) { + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { + if (only_route_lanes) { + break; + } + const auto next_lanes = getNextLanelets(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); + } + // loop check + if (lanelet.id() == next_lanelet.id()) { + break; + } + lanelet_sequence_forward.push_back(next_lanelet); + current_lanelet = next_lanelet; + length += + static_cast(boost::geometry::length(next_lanelet.centerline().basicLineString())); + } + + return lanelet_sequence_forward; +} + +lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, const double min_length, const bool only_route_lanes) const +{ + lanelet::ConstLanelets lanelet_sequence_backward; + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { + return lanelet_sequence_backward; + } + + lanelet::ConstLanelet current_lanelet = lanelet; + double length = 0; + lanelet::ConstLanelets previous_lanelets; + + auto checkForLoop = + [&lanelet](const lanelet::ConstLanelets & lanelets_to_check, const bool is_route_lanelets) { + if (is_route_lanelets) { + return std::none_of( + lanelets_to_check.begin(), lanelets_to_check.end(), + [lanelet](auto & prev_llt) { return lanelet.id() != prev_llt.id(); }); + } + return std::any_of( + lanelets_to_check.begin(), lanelets_to_check.end(), + [lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); }); + }; + + auto isNewLanelet = [&lanelet, + &lanelet_sequence_backward](const lanelet::ConstLanelet & lanelet_to_check) { + if (lanelet.id() == lanelet_to_check.id()) return false; + return std::none_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [&lanelet_to_check](auto & backward) { return (backward.id() == lanelet_to_check.id()); }); + }; + + while (rclcpp::ok() && length < min_length) { + previous_lanelets.clear(); + bool is_route_lanelets = true; + if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) { + if (only_route_lanes) break; + previous_lanelets = getPreviousLanelets(current_lanelet); + if (previous_lanelets.empty()) break; + is_route_lanelets = false; + } + + if (checkForLoop(previous_lanelets, is_route_lanelets)) break; + + for (const auto & prev_lanelet : previous_lanelets) { + if (!isNewLanelet(prev_lanelet) || exists(goal_lanelets_, prev_lanelet)) continue; + lanelet_sequence_backward.push_back(prev_lanelet); + length += + static_cast(boost::geometry::length(prev_lanelet.centerline().basicLineString())); + current_lanelet = prev_lanelet; + break; + } + } + + std::reverse(lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); + return lanelet_sequence_backward; +} + +lanelet::ConstLanelets RouteHandler::getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const double backward_distance, + const double forward_distance, const bool only_route_lanes) const +{ + Pose current_pose{}; + current_pose.orientation.w = 1; + if (!lanelet.centerline().empty()) { + current_pose.position = lanelet::utils::conversion::toGeomMsgPt(lanelet.centerline().front()); + } + + lanelet::ConstLanelets lanelet_sequence; + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { + return lanelet_sequence; + } + + const lanelet::ConstLanelets lanelet_sequence_forward = std::invoke([&]() { + if (only_route_lanes) { + return getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + } else if (isShoulderLanelet(lanelet)) { + return getShoulderLaneletSequenceAfter(lanelet, forward_distance); + } + return lanelet::ConstLanelets{}; + }); + const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { + const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + if (arc_coordinate.length < backward_distance) { + if (only_route_lanes) { + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + } else if (isShoulderLanelet(lanelet)) { + return getShoulderLaneletSequenceUpTo(lanelet, backward_distance); + } + } + return lanelet::ConstLanelets{}; + }); + + // loop check + if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) { + if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) { + return lanelet_sequence_forward; + } + } + lanelet_sequence.insert( + lanelet_sequence.end(), lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); + lanelet_sequence.push_back(lanelet); + lanelet_sequence.insert( + lanelet_sequence.end(), lanelet_sequence_forward.begin(), lanelet_sequence_forward.end()); + + return lanelet_sequence; +} + +lanelet::ConstLanelets RouteHandler::getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance, + const double forward_distance, const bool only_route_lanes) const +{ + if (only_route_lanes && !exists(route_lanelets_, lanelet)) { + return {}; + } + + lanelet::ConstLanelets lanelet_sequence_forward = + getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + lanelet::ConstLanelets lanelet_sequence = std::invoke([&]() { + const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + if (arc_coordinate.length < backward_distance) { + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + } + return lanelet::ConstLanelets{}; + }); + + // loop check + if (!lanelet_sequence_forward.empty() && !lanelet_sequence.empty()) { + if (lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) { + return lanelet_sequence_forward; + } + } + lanelet_sequence.push_back(lanelet); + std::move( + lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(), + std::back_inserter(lanelet_sequence)); + return lanelet_sequence; +} + +lanelet::ConstLanelets RouteHandler::getRoadLaneletsAtPose(const Pose & pose) const +{ + lanelet::ConstLanelets road_lanelets_at_pose; + const lanelet::BasicPoint2d p{pose.position.x, pose.position.y}; + const auto lanelets_at_pose = lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p)); + for (const auto & lanelet_at_pose : lanelets_at_pose) { + // confirm that the pose is inside the lanelet since "search" does an approximation with boxes + const auto is_pose_inside_lanelet = lanelet::geometry::inside(lanelet_at_pose, p); + if (is_pose_inside_lanelet && isRoadLanelet(lanelet_at_pose)) + road_lanelets_at_pose.push_back(lanelet_at_pose); + } + return road_lanelets_at_pose; +} + +std::optional RouteHandler::getFollowingShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const +{ + bool found = false; + const auto & search_point = lanelet.centerline().back().basicPoint2d(); + const auto next_lanelet = lanelet_map_ptr_->laneletLayer.nearestUntil( + search_point, [&](const auto & bbox, const auto & ll) { + if (isShoulderLanelet(ll) && lanelet::geometry::follows(lanelet, ll)) found = true; + // stop search once next shoulder lanelet is found, or the bbox does not touch the search + // point + return found || lanelet::geometry::distance2d(bbox, search_point) > 1e-3; + }); + if (found && next_lanelet.has_value()) return *next_lanelet; + return std::nullopt; +} + +std::optional RouteHandler::getLeftShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const +{ + for (const auto & other_lanelet : + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound())) { + if (other_lanelet.rightBound() == lanelet.leftBound() && isShoulderLanelet(other_lanelet)) + return other_lanelet; + } + return std::nullopt; +} + +std::optional RouteHandler::getRightShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const +{ + for (const auto & other_lanelet : + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound())) { + if (other_lanelet.leftBound() == lanelet.rightBound() && isShoulderLanelet(other_lanelet)) + return other_lanelet; + } + return std::nullopt; +} + +lanelet::ConstLanelets RouteHandler::getShoulderLaneletsAtPose(const Pose & pose) const +{ + lanelet::ConstLanelets lanelets_at_pose; + const lanelet::BasicPoint2d p{pose.position.x, pose.position.y}; + const auto candidates_at_pose = lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p)); + for (const auto & candidate : candidates_at_pose) { + // confirm that the pose is inside the lanelet since "search" does an approximation with boxes + const auto is_pose_inside_lanelet = lanelet::geometry::inside(candidate, p); + if (is_pose_inside_lanelet && isShoulderLanelet(candidate)) + lanelets_at_pose.push_back(candidate); + } + return lanelets_at_pose; +} + +lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, const double min_length) const +{ + lanelet::ConstLanelets lanelet_sequence_forward; + if (!isShoulderLanelet(lanelet)) return lanelet_sequence_forward; + + double length = 0; + lanelet::ConstLanelet current_lanelet = lanelet; + std::set searched_ids{}; + while (rclcpp::ok() && length < min_length) { + const auto next_lanelet = getFollowingShoulderLanelet(current_lanelet); + if (!next_lanelet) break; + lanelet_sequence_forward.push_back(*next_lanelet); + if (searched_ids.find(next_lanelet->id()) != searched_ids.end()) { + // loop shoulder detected + break; + } + searched_ids.insert(next_lanelet->id()); + current_lanelet = *next_lanelet; + length += + static_cast(boost::geometry::length(next_lanelet->centerline().basicLineString())); + } + + return lanelet_sequence_forward; +} + +std::optional RouteHandler::getPreviousShoulderLanelet( + const lanelet::ConstLanelet & lanelet) const +{ + bool found = false; + const auto & search_point = lanelet.centerline().front().basicPoint2d(); + const auto previous_lanelet = lanelet_map_ptr_->laneletLayer.nearestUntil( + search_point, [&](const auto & bbox, const auto & ll) { + if (isShoulderLanelet(ll) && lanelet::geometry::follows(ll, lanelet)) found = true; + // stop search once prev shoulder lanelet is found, or the bbox does not touch the search + // point + return found || lanelet::geometry::distance2d(bbox, search_point) > 1e-3; + }); + if (found && previous_lanelet.has_value()) return *previous_lanelet; + return std::nullopt; +} + +lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, const double min_length) const +{ + lanelet::ConstLanelets lanelet_sequence_backward; + if (!isShoulderLanelet(lanelet)) return lanelet_sequence_backward; + + double length = 0; + lanelet::ConstLanelet current_lanelet = lanelet; + std::set searched_ids{}; + while (rclcpp::ok() && length < min_length) { + const auto prev_lanelet = getPreviousShoulderLanelet(current_lanelet); + if (!prev_lanelet) break; + + lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), *prev_lanelet); + if (searched_ids.find(prev_lanelet->id()) != searched_ids.end()) { + // loop shoulder detected + break; + } + searched_ids.insert(prev_lanelet->id()); + current_lanelet = *prev_lanelet; + length += + static_cast(boost::geometry::length(prev_lanelet->centerline().basicLineString())); + } + + return lanelet_sequence_backward; +} + +lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequence( + const lanelet::ConstLanelet & lanelet, const Pose & pose, const double backward_distance, + const double forward_distance) const +{ + lanelet::ConstLanelets lanelet_sequence; + if (!isShoulderLanelet(lanelet)) { + return lanelet_sequence; + } + + lanelet::ConstLanelets lanelet_sequence_forward = + getShoulderLaneletSequenceAfter(lanelet, forward_distance); + const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { + const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, pose); + if (arc_coordinate.length < backward_distance) { + return getShoulderLaneletSequenceUpTo(lanelet, backward_distance); + } + return lanelet::ConstLanelets{}; + }); + + // loop check + if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) { + if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) { + return lanelet_sequence_forward; + } + } + lanelet_sequence.insert( + lanelet_sequence.end(), lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); + + lanelet_sequence.push_back(lanelet); + lanelet_sequence.insert( + lanelet_sequence.end(), lanelet_sequence_forward.begin(), lanelet_sequence_forward.end()); + + return lanelet_sequence; +} + +bool RouteHandler::getClosestLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); +} + +bool RouteHandler::getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + +bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, + const double yaw_threshold) const +{ + return lanelet::utils::query::getClosestLaneletWithConstrains( + route_lanelets_, search_pose, closest_lanelet, dist_threshold, yaw_threshold); +} + +bool RouteHandler::getClosestRouteLaneletFromLanelet( + const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet, + lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, + const double yaw_threshold) const +{ + lanelet::ConstLanelets previous_lanelets, next_lanelets, lanelet_sequence; + if (getPreviousLaneletsWithinRoute(reference_lanelet, &previous_lanelets)) { + lanelet_sequence = previous_lanelets; + } + + lanelet_sequence.push_back(reference_lanelet); + + if (getNextLaneletsWithinRoute(reference_lanelet, &next_lanelets)) { + lanelet_sequence.insert(lanelet_sequence.end(), next_lanelets.begin(), next_lanelets.end()); + } + + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, search_pose, closest_lanelet, dist_threshold, yaw_threshold)) { + return true; + } + + return false; +} + +bool RouteHandler::getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const +{ + if (exists(goal_lanelets_, lanelet)) { + return false; + } + + const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id; + + const auto following_lanelets = routing_graph_ptr_->following(lanelet); + next_lanelets->clear(); + for (const auto & llt : following_lanelets) { + if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) { + next_lanelets->push_back(llt); + } + } + return !(next_lanelets->empty()); +} + +bool RouteHandler::getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const +{ + lanelet::ConstLanelets next_lanelets{}; + if (getNextLaneletsWithinRoute(lanelet, &next_lanelets)) { + *next_lanelet = next_lanelets.front(); + return true; + } + return false; +} + +lanelet::ConstLanelets RouteHandler::getNextLanelets(const lanelet::ConstLanelet & lanelet) const +{ + return routing_graph_ptr_->following(lanelet); +} + +bool RouteHandler::getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const +{ + if (exists(start_lanelets_, lanelet)) { + return false; + } + const auto candidate_lanelets = routing_graph_ptr_->previous(lanelet); + prev_lanelets->clear(); + for (const auto & llt : candidate_lanelets) { + if (exists(route_lanelets_, llt)) { + prev_lanelets->push_back(llt); + } + } + return !(prev_lanelets->empty()); +} + +lanelet::ConstLanelets RouteHandler::getPreviousLanelets( + const lanelet::ConstLanelet & lanelet) const +{ + return routing_graph_ptr_->previous(lanelet); +} + +std::optional RouteHandler::getRightLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const +{ + // right road lanelet of shoulder lanelet + if (isShoulderLanelet(lanelet)) { + const auto right_lanelets = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound()); + for (const auto & right_lanelet : right_lanelets) + if (isRoadLanelet(right_lanelet)) return right_lanelet; + return std::nullopt; + } + + // right shoulder lanelet + if (get_shoulder_lane) { + const auto right_shoulder_lanelet = getRightShoulderLanelet(lanelet); + if (right_shoulder_lanelet) return *right_shoulder_lanelet; + } + + // routable lane + const auto & right_lane = routing_graph_ptr_->right(lanelet); + if (right_lane) { + return *right_lane; + } + + // non-routable lane (e.g. lane change infeasible) + const auto & adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet); + if (adjacent_right_lane) { + return *adjacent_right_lane; + } + + // same root right lanelet + if (!enable_same_root) { + return std::nullopt; + } + + lanelet::ConstLanelets prev_lanelet; + if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + if (lanelet.rightBound().back().id() == lane.leftBound().back().id()) { + return lane; + } + } + return std::nullopt; + } + + const auto next_right_lane = getRightLanelet(next_lanelet, false); + if (!next_right_lane) { + return std::nullopt; + } + + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + for (const auto & target_lane : getNextLanelets(lane)) { + if (next_right_lane.value().id() == target_lane.id()) { + return lane; + } + } + } + + return std::nullopt; +} + +std::optional RouteHandler::getLeftLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const +{ + // left road lanelet of shoulder lanelet + if (isShoulderLanelet(lanelet)) { + const auto left_lanelets = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound()); + for (const auto & left_lanelet : left_lanelets) + if (isRoadLanelet(left_lanelet)) return left_lanelet; + return std::nullopt; + } + + // left shoulder lanelet + if (get_shoulder_lane) { + const auto left_shoulder_lanelet = getLeftShoulderLanelet(lanelet); + if (left_shoulder_lanelet) return *left_shoulder_lanelet; + } + + // routable lane + const auto & left_lane = routing_graph_ptr_->left(lanelet); + if (left_lane) { + return *left_lane; + } + + // non-routable lane (e.g. lane change infeasible) + const auto & adjacent_left_lane = routing_graph_ptr_->adjacentLeft(lanelet); + if (adjacent_left_lane) { + return *adjacent_left_lane; + } + + // same root right lanelet + if (!enable_same_root) { + return std::nullopt; + } + + lanelet::ConstLanelets prev_lanelet; + if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + if (lanelet.leftBound().back().id() == lane.rightBound().back().id()) { + return lane; + } + } + return std::nullopt; + } + + const auto next_left_lane = getLeftLanelet(next_lanelet, false); + if (!next_left_lane) { + return std::nullopt; + } + + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + for (const auto & target_lane : getNextLanelets(lane)) { + if (next_left_lane.value().id() == target_lane.id()) { + return lane; + } + } + } + + return std::nullopt; +} + +lanelet::Lanelets RouteHandler::getRightOppositeLanelets( + const lanelet::ConstLanelet & lanelet) const +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} + +lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite, + const bool & invert_opposite) const noexcept +{ + lanelet::ConstLanelets linestring_shared; + try { + auto lanelet_at_left = getLeftLanelet(lane); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); + while (lanelet_at_left) { + linestring_shared.push_back(lanelet_at_left.value()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); + if (!lanelet_at_left) { + break; + } + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); + } + + if (!lanelet_at_left_opposite.empty() && include_opposite) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); + } else { + linestring_shared.push_back(lanelet_at_left_opposite.front()); + } + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); + while (lanelet_at_right) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_right.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_right.value()); + } + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); + } + } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllLeftSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllLeftSharedLinestringLanelets" << std::endl; + return {}; + } + return linestring_shared; +} + +lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite, + const bool & invert_opposite) const noexcept +{ + lanelet::ConstLanelets linestring_shared; + try { + auto lanelet_at_right = getRightLanelet(lane); + auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); + while (lanelet_at_right) { + linestring_shared.push_back(lanelet_at_right.value()); + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); + if (!lanelet_at_right) { + break; + } + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); + } + + if (!lanelet_at_right_opposite.empty() && include_opposite) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); + } else { + linestring_shared.push_back(lanelet_at_right_opposite.front()); + } + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_left.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_left.value()); + } + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); + } + } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllRightSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllRightSharedLinestringLanelets" << std::endl; + return {}; + } + return linestring_shared; +} + +lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left, bool is_opposite, + const bool & invert_opposite) const noexcept +{ + lanelet::ConstLanelets shared{current_lane}; + + if (is_right) { + const lanelet::ConstLanelets all_right_lanelets = + getAllRightSharedLinestringLanelets(current_lane, is_opposite, invert_opposite); + shared.insert(shared.end(), all_right_lanelets.begin(), all_right_lanelets.end()); + } + + if (is_left) { + const lanelet::ConstLanelets all_left_lanelets = + getAllLeftSharedLinestringLanelets(current_lane, is_opposite, invert_opposite); + shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end()); + } + + return shared; +} + +lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} + +lanelet::ConstLanelet RouteHandler::getMostRightLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const +{ + // recursively compute the width of the lanes + const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane); + + if (same) { + return getMostRightLanelet(same.value(), enable_same_root, get_shoulder_lane); + } + + return lanelet; +} + +lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const +{ + // recursively compute the width of the lanes + const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane); + + if (same) { + return getMostLeftLanelet(same.value(), enable_same_root, get_shoulder_lane); + } + + return lanelet; +} + +std::vector RouteHandler::getPrecedingLaneletSequence( + const lanelet::ConstLanelet & lanelet, const double length, + const lanelet::ConstLanelets & exclude_lanelets) const +{ + return lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr_, lanelet, length, exclude_lanelets); +} + +std::optional RouteHandler::getLaneChangeTarget( + const lanelet::ConstLanelets & lanelets, const Direction direction) const +{ + for (const auto & lanelet : lanelets) { + const int num = getNumLaneToPreferredLane(lanelet, direction); + if (num == 0) { + continue; + } + + if (direction == Direction::NONE || direction == Direction::RIGHT) { + if (num < 0) { + const auto right_lanes = routing_graph_ptr_->right(lanelet); + if (!!right_lanes) { + return *right_lanes; + } + } + } + + if (direction == Direction::NONE || direction == Direction::LEFT) { + if (num > 0) { + const auto left_lanes = routing_graph_ptr_->left(lanelet); + if (!!left_lanes) { + return *left_lanes; + } + } + } + } + + return std::nullopt; +} + +std::optional RouteHandler::getLaneChangeTargetExceptPreferredLane( + const lanelet::ConstLanelets & lanelets, const Direction direction) const +{ + for (const auto & lanelet : lanelets) { + if (direction == Direction::RIGHT) { + // Get right lanelet if preferred lane is on the left + if (getNumLaneToPreferredLane(lanelet, direction) < 0) { + continue; + } + + const auto right_lanes = routing_graph_ptr_->right(lanelet); + if (!!right_lanes) { + return *right_lanes; + } + } + + if (direction == Direction::LEFT) { + // Get left lanelet if preferred lane is on the right + if (getNumLaneToPreferredLane(lanelet, direction) > 0) { + continue; + } + const auto left_lanes = routing_graph_ptr_->left(lanelet); + if (!!left_lanes) { + return *left_lanes; + } + } + } + + return std::nullopt; +} + +std::optional RouteHandler::getPullOverTarget(const Pose & goal_pose) const +{ + const lanelet::BasicPoint2d p(goal_pose.position.x, goal_pose.position.y); + constexpr auto search_distance = 0.1; + const lanelet::BasicPoint2d offset(search_distance, search_distance); + const auto lanelets_in_range = + lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p - offset, p + offset)); + for (const auto & lanelet : lanelets_in_range) { + const auto is_in_lanelet = lanelet::utils::isInLanelet(goal_pose, lanelet, search_distance); + if (is_in_lanelet && isShoulderLanelet(lanelet)) return lanelet; + } + return std::nullopt; +} + +std::optional RouteHandler::getPullOutStartLane( + const Pose & pose, const double vehicle_width) const +{ + const lanelet::BasicPoint2d p(pose.position.x, pose.position.y); + const auto search_distance = vehicle_width / 2.0; + const lanelet::BasicPoint2d offset(search_distance, search_distance); + const auto lanelets_in_range = + lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p - offset, p + offset)); + for (const auto & lanelet : lanelets_in_range) { + const auto is_in_lanelet = lanelet::utils::isInLanelet(pose, lanelet, search_distance); + if (is_in_lanelet && isShoulderLanelet(lanelet)) return lanelet; + } + return std::nullopt; +} + +int RouteHandler::getNumLaneToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction) const +{ + if (exists(preferred_lanelets_, lanelet)) { + return 0; + } + + if ((direction == Direction::NONE) || (direction == Direction::RIGHT)) { + int num{0}; + const auto & right_lanes = + lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); + for (const auto & right : right_lanes) { + num--; + if (exists(preferred_lanelets_, right)) { + return num; + } + } + } + + if ((direction == Direction::NONE) || (direction == Direction::LEFT)) { + const auto & left_lanes = + lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); + int num = 0; + for (const auto & left : left_lanes) { + num++; + if (exists(preferred_lanelets_, left)) { + return num; + } + } + } + + return 0; // TODO(Horibe) check if return 0 is appropriate. +} + +std::vector RouteHandler::getLateralIntervalsToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction) const +{ + if (exists(preferred_lanelets_, lanelet)) { + return {}; + } + + if ((direction == Direction::NONE) || (direction == Direction::RIGHT)) { + std::vector intervals; + lanelet::ConstLanelet current_lanelet = lanelet; + const auto & right_lanes = + lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); + for (const auto & right : right_lanes) { + const auto & current_centerline = current_lanelet.centerline(); + const auto & next_centerline = right.centerline(); + if (current_centerline.empty() || next_centerline.empty()) { + return intervals; + } + const auto & curr_pt = current_centerline.front(); + const auto & next_pt = next_centerline.front(); + intervals.push_back(-lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt))); + + if (exists(preferred_lanelets_, right)) { + return intervals; + } + current_lanelet = right; + } + } + + if ((direction == Direction::NONE) || (direction == Direction::LEFT)) { + std::vector intervals; + lanelet::ConstLanelet current_lanelet = lanelet; + const auto & left_lanes = + lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); + for (const auto & left : left_lanes) { + const auto & current_centerline = current_lanelet.centerline(); + const auto & next_centerline = left.centerline(); + if (current_centerline.empty() || next_centerline.empty()) { + return intervals; + } + const auto & curr_pt = current_centerline.front(); + const auto & next_pt = next_centerline.front(); + intervals.push_back(lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt))); + + if (exists(preferred_lanelets_, left)) { + return intervals; + } + current_lanelet = left; + } + } + + return {}; +} + +PathWithLaneId RouteHandler::getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, + bool use_exact) const +{ + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + + // 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( + ReferencePoint{false, lanelet::utils::conversion::toGeomMsgPt(pt)}); + }; + double s = 0; + for (const auto & llt : lanelet_sequence) { + piecewise_ref_points_vec.push_back(std::vector{}); + + 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 = + (i + 1 < centerline.size()) ? centerline[i + 1] : centerline[i]; + const double distance = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt)); + + if (s < s_start && s + distance > s_start) { + const auto p = use_exact ? get3DPointFrom2DArcLength(lanelet_sequence, s_start) : pt; + add_ref_point(p); + } + if (s >= s_start && s <= s_end) { + 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_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 piecewise_waypoints_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 + 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 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 = 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; + } + } + } + + // 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 + if (reference_path.points.size() == 1) { + const lanelet::Id lane_id = reference_path.points.front().lane_ids.front(); + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id); + const auto point = reference_path.points.front().point.pose.position; + const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); + PathPointWithLaneId path_point{}; + path_point.lane_ids.push_back(lane_id); + constexpr double ds{0.1}; + path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); + path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); + path_point.point.pose.position.z = point.z; + reference_path.points.push_back(path_point); + } + + // set angle + for (size_t i = 0; i < reference_path.points.size(); i++) { + double angle{0.0}; + const auto & pts = reference_path.points; + if (i + 1 < reference_path.points.size()) { + angle = autoware_utils::calc_azimuth_angle( + pts.at(i).point.pose.position, pts.at(i + 1).point.pose.position); + } else if (i != 0) { + angle = autoware_utils::calc_azimuth_angle( + pts.at(i - 1).point.pose.position, pts.at(i).point.pose.position); + } + reference_path.points.at(i).point.pose.orientation = + autoware_utils::create_quaternion_from_yaw(angle); + } + + 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 piecewise_waypoints_lanelet_sequence_index, + const bool is_removing_direction_forward) const +{ + const double waypoints_interpolation_arc_margin_ratio = 10.0; + + // calculate arc length threshold + const double front_arc_length_threshold = [&]() { + 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(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(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( + 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_unsigned_index = 0; + ref_point_unsigned_index < target_piecewise_ref_points.size(); + ++ref_point_unsigned_index) { + const size_t ref_point_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_removing_direction_forward ? 0 : -target_lanelet_arc_length) + + calcArcCoordinates(lanelet_sequence.at(target_lanelet_sequence_index), ref_point.point) + .length; + if (is_removing_direction_forward) { + if (back_arc_length_threshold < offset_arc_length + ref_point_arc_length) { + return true; + } + } else { + if (offset_arc_length + ref_point_arc_length < front_arc_length_threshold) { + 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_removing_direction_forward ? 1 : -1; + offset_arc_length = (is_removing_direction_forward ? 1 : -1) * target_lanelet_arc_length; + } +} + +bool RouteHandler::isMapMsgReady() const +{ + return is_map_msg_ready_; +} + +lanelet::routing::RoutingGraphPtr RouteHandler::getRoutingGraphPtr() const +{ + return routing_graph_ptr_; +} + +lanelet::traffic_rules::TrafficRulesPtr RouteHandler::getTrafficRulesPtr() const +{ + return traffic_rules_ptr_; +} + +std::shared_ptr RouteHandler::getOverallGraphPtr() + const +{ + return overall_graphs_ptr_; +} + +lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const +{ + return lanelet_map_ptr_; +} + +bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const +{ + return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + lanelet.attribute(lanelet::AttributeName::Subtype) == "road_shoulder"; +} + +bool RouteHandler::isRouteLanelet(const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::contains(route_lanelets_, lanelet); +} + +bool RouteHandler::isRoadLanelet(const lanelet::ConstLanelet & lanelet) const +{ + return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + lanelet.attribute(lanelet::AttributeName::Subtype) == lanelet::AttributeValueString::Road; +} + +lanelet::ConstLanelets RouteHandler::getPreviousLaneletSequence( + const lanelet::ConstLanelets & lanelet_sequence) const +{ + lanelet::ConstLanelets previous_lanelet_sequence; + if (lanelet_sequence.empty()) { + return previous_lanelet_sequence; + } + + const auto & first_lane = lanelet_sequence.front(); + if (exists(start_lanelets_, first_lane)) { + return previous_lanelet_sequence; + } + + auto right_relations = + lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, first_lane); + for (const auto & right : right_relations) { + previous_lanelet_sequence = getLaneletSequenceUpTo(right); + if (!previous_lanelet_sequence.empty()) { + return previous_lanelet_sequence; + } + } + + auto left_relations = lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, first_lane); + for (const auto & left : left_relations) { + previous_lanelet_sequence = getLaneletSequenceUpTo(left); + if (!previous_lanelet_sequence.empty()) { + return previous_lanelet_sequence; + } + } + return previous_lanelet_sequence; +} + +lanelet::ConstLanelets RouteHandler::getNeighborsWithinRoute( + const lanelet::ConstLanelet & lanelet) const +{ + const lanelet::ConstLanelets neighbor_lanelets = + lanelet::utils::query::getAllNeighbors(routing_graph_ptr_, lanelet); + lanelet::ConstLanelets neighbors_within_route; + for (const auto & llt : neighbor_lanelets) { + if (exists(route_lanelets_, llt)) { + neighbors_within_route.push_back(llt); + } + } + return neighbors_within_route; +} + +bool RouteHandler::planPathLaneletsBetweenCheckpoints( + const Pose & start_checkpoint, const Pose & goal_checkpoint, + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const +{ + // Find lanelets for start point. First, find all lanelets containing the start point to calculate + // all possible route later. It fails when the point is not located on any road lanelet (e.g. the + // start point is located out of any lanelets or road_shoulder lanelet which is not contained in + // road lanelet). In that case, find the closest lanelet instead (within some maximum range). + constexpr auto max_search_range = 20.0; + auto start_lanelets = getRoadLaneletsAtPose(start_checkpoint); + lanelet::ConstLanelet start_lanelet; + if (start_lanelets.empty()) { + const lanelet::BasicPoint2d p(start_checkpoint.position.x, start_checkpoint.position.y); + const lanelet::BoundingBox2d bbox( + lanelet::BasicPoint2d(p.x() - max_search_range, p.y() - max_search_range), + lanelet::BasicPoint2d(p.x() + max_search_range, p.y() + max_search_range)); + // std::as_const(*ptr) to use the const version of the search function + auto candidates = std::as_const(*lanelet_map_ptr_).laneletLayer.search(bbox); + candidates.erase( + std::remove_if( + candidates.begin(), candidates.end(), [&](const auto & l) { return !isRoadLanelet(l); }), + candidates.end()); + if (lanelet::utils::query::getClosestLanelet(candidates, start_checkpoint, &start_lanelet)) + start_lanelets = {start_lanelet}; + } + if (start_lanelets.empty()) { + RCLCPP_WARN_STREAM( + logger_, "Failed to find current lanelet." + << std::endl + << " - start checkpoint: " << toString(start_checkpoint) << std::endl + << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl); + return false; + } + + // Find lanelets for goal point. + lanelet::ConstLanelet goal_lanelet; + const lanelet::BasicPoint2d p(goal_checkpoint.position.x, goal_checkpoint.position.y); + const lanelet::BoundingBox2d bbox( + lanelet::BasicPoint2d(p.x() - max_search_range, p.y() - max_search_range), + lanelet::BasicPoint2d(p.x() + max_search_range, p.y() + max_search_range)); + auto candidates = std::as_const(*lanelet_map_ptr_).laneletLayer.search(bbox); + candidates.erase( + std::remove_if( + candidates.begin(), candidates.end(), [&](const auto & l) { return !isRoadLanelet(l); }), + candidates.end()); + // if there is a lanelet in candidates that is included in previous preferred lanelets, + // set it as goal_lanelet. + // this is to select the same lane as much as possible when rerouting with waypoints. + const auto findGoalClosestPreferredLanelet = [&]() -> std::optional { + lanelet::ConstLanelet closest_lanelet; + if (getClosestPreferredLaneletWithinRoute(goal_checkpoint, &closest_lanelet)) { + if (std::find(candidates.begin(), candidates.end(), closest_lanelet) != candidates.end()) { + if (lanelet::utils::isInLanelet(goal_checkpoint, closest_lanelet)) { + return closest_lanelet; + } + } + } + if (getClosestLaneletWithinRoute(goal_checkpoint, &closest_lanelet)) { + if (std::find(candidates.begin(), candidates.end(), closest_lanelet) != candidates.end()) { + if (lanelet::utils::isInLanelet(goal_checkpoint, closest_lanelet)) { + std::stringstream preferred_lanelets_str; + for (const auto & preferred_lanelet : preferred_lanelets_) { + preferred_lanelets_str << preferred_lanelet.id() << ", "; + } + RCLCPP_WARN( + logger_, + "Failed to find reroute on previous preferred lanelets %s, but on previous route " + "segment %ld still", + preferred_lanelets_str.str().c_str(), closest_lanelet.id()); + return closest_lanelet; + } + } + } + return std::nullopt; + }; + if (auto closest_lanelet = findGoalClosestPreferredLanelet()) { + goal_lanelet = closest_lanelet.value(); + } else { + if (!lanelet::utils::query::getClosestLanelet(candidates, goal_checkpoint, &goal_lanelet)) { + RCLCPP_WARN_STREAM( + logger_, "Failed to find closest lanelet." + << std::endl + << " - start checkpoint: " << toString(start_checkpoint) << std::endl + << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl); + return false; + } + } + + lanelet::Optional optional_route; + lanelet::routing::LaneletPath shortest_path; + bool is_route_found = false; + + double smallest_angle_diff = std::numeric_limits::max(); + constexpr double yaw_threshold = M_PI / 2.0; + + for (const auto & st_llt : start_lanelets) { + // check if the angle difference between start_checkpoint and start lanelet center line + // orientation is in yaw_threshold range + double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position); + double pose_yaw = tf2::getYaw(start_checkpoint.orientation); + double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); + + bool is_proper_angle = angle_diff <= std::abs(yaw_threshold); + + optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0); + if (!optional_route || !is_proper_angle) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to find a proper route!" + << std::endl + << " - start checkpoint: " << toString(start_checkpoint) << std::endl + << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl + << " - start lane id: " << st_llt.id() << std::endl + << " - goal lane id: " << goal_lanelet.id() << std::endl); + continue; + } + is_route_found = true; + lanelet::ConstLanelet preferred_lane{}; + if (getClosestPreferredLaneletWithinRoute(start_checkpoint, &preferred_lane)) { + if (st_llt.id() == preferred_lane.id()) { + shortest_path = optional_route->shortestPath(); + start_lanelet = st_llt; + break; + } + } + if (angle_diff < smallest_angle_diff) { + smallest_angle_diff = angle_diff; + shortest_path = optional_route->shortestPath(); + start_lanelet = st_llt; + } + } + + if (is_route_found) { + lanelet::routing::LaneletPath path; + path = [&]() -> lanelet::routing::LaneletPath { + if (consider_no_drivable_lanes && hasNoDrivableLaneInPath(shortest_path)) { + const auto drivable_lane_path = findDrivableLanePath(start_lanelet, goal_lanelet); + if (drivable_lane_path) return *drivable_lane_path; + } + return shortest_path; + }(); + + path_lanelets->reserve(path.size()); + for (const auto & llt : path) { + path_lanelets->push_back(llt); + } + } + + return is_route_found; +} + +std::vector RouteHandler::createMapSegments( + const lanelet::ConstLanelets & path_lanelets) const +{ + const auto main_path = getMainLanelets(path_lanelets); + + std::vector route_sections; + + if (main_path.empty()) { + return route_sections; + } + + route_sections.reserve(main_path.size()); + for (const auto & main_llt : main_path) { + LaneletSegment route_section_msg; + const lanelet::ConstLanelets route_section_lanelets = getNeighborsWithinRoute(main_llt); + route_section_msg.preferred_primitive.id = main_llt.id(); + route_section_msg.primitives.reserve(route_section_lanelets.size()); + for (const auto & section_llt : route_section_lanelets) { + LaneletPrimitive p; + p.id = section_llt.id(); + p.primitive_type = "lane"; + route_section_msg.primitives.push_back(p); + } + route_sections.push_back(route_section_msg); + } + return route_sections; +} + +lanelet::ConstLanelets RouteHandler::getMainLanelets( + const lanelet::ConstLanelets & path_lanelets) const +{ + auto lanelet_sequence = getLaneletSequence(path_lanelets.back()); + + RCLCPP_INFO_STREAM(logger_, "getMainLanelets: lanelet_sequence = " << lanelet_sequence); + + lanelet::ConstLanelets main_lanelets; + while (!lanelet_sequence.empty()) { + main_lanelets.insert(main_lanelets.begin(), lanelet_sequence.begin(), lanelet_sequence.end()); + lanelet_sequence = getPreviousLaneletSequence(lanelet_sequence); + } + return main_lanelets; +} + +bool RouteHandler::isNoDrivableLane(const lanelet::ConstLanelet & llt) +{ + const std::string no_drivable_lane_attribute = llt.attributeOr("no_drivable_lane", "no"); + return no_drivable_lane_attribute == "yes"; +} + +bool RouteHandler::hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & path) const +{ + for (const auto & llt : path) + if (isNoDrivableLane(llt)) return true; + return false; +} + +std::optional RouteHandler::findDrivableLanePath( + const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet) const +{ + // we create a new routing graph with infinite cost on no drivable lanes + const auto drivable_routing_graph_ptr = lanelet::routing::RoutingGraph::build( + *lanelet_map_ptr_, *traffic_rules_ptr_, + lanelet::routing::RoutingCostPtrs{std::make_shared()}); + const auto route = drivable_routing_graph_ptr->getRoute(start_lanelet, goal_lanelet, 0); + if (route) return route->shortestPath(); + return {}; +} + +Pose RouteHandler::get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const +{ + double accumulated_distance2d = 0; + for (const auto & llt : lanelet_sequence) { + const auto & centerline = llt.centerline(); + for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) { + const auto pt = *it; + const auto next_pt = *std::next(it); + const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt)); + if (accumulated_distance2d + distance2d > s) { + const double ratio = (s - accumulated_distance2d) / distance2d; + const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio; + const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x()); + Pose pose; + pose.position = create_point(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + pose.orientation = create_quaternion_from_yaw(yaw); + return pose; + } + accumulated_distance2d += distance2d; + } + } + return Pose{}; +} +} // namespace autoware::route_handler diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp new file mode 100644 index 0000000000..3fcb590d36 --- /dev/null +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -0,0 +1,315 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_route_handler.hpp" + +#include +#include + +#include + +namespace autoware::route_handler::test +{ +TEST_F(TestRouteHandler, isRouteHandlerReadyTest) +{ + ASSERT_TRUE(route_handler_->isHandlerReady()); +} + +TEST_F(TestRouteHandler, checkIfIDReturned) +{ + const auto lanelet1 = route_handler_->getLaneletsFromId(4785); + const auto is_lanelet1_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet1); + ASSERT_TRUE(is_lanelet1_in_goal_route_section); + + const auto lanelet2 = route_handler_->getLaneletsFromId(4780); + const auto is_lanelet2_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet2); + ASSERT_FALSE(is_lanelet2_in_goal_route_section); +} + +TEST_F(TestRouteHandler, getGoalLaneId) +{ + lanelet::ConstLanelet goal_lane; + + const auto goal_lane_obtained = route_handler_->getGoalLanelet(&goal_lane); + ASSERT_TRUE(goal_lane_obtained); + ASSERT_EQ(goal_lane.id(), 5088); +} + +TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) +{ + set_route_handler("overlap_map.osm"); + ASSERT_FALSE(route_handler_->isHandlerReady()); + + geometry_msgs::msg::Pose start_pose; + geometry_msgs::msg::Pose goal_pose; + start_pose.position = autoware_utils::create_point(3728.870361, 73739.281250, 0); + start_pose.orientation = autoware_utils::create_quaternion(0, 0, -0.513117, 0.858319); + goal_pose.position = autoware_utils::create_point(3729.961182, 73727.328125, 0); + goal_pose.orientation = autoware_utils::create_quaternion(0, 0, 0.234831, 0.972036); + + lanelet::ConstLanelets path_lanelets; + ASSERT_TRUE( + route_handler_->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)); + ASSERT_EQ(path_lanelets.size(), 12); + ASSERT_EQ(path_lanelets.front().id(), 168); + ASSERT_EQ(path_lanelets.back().id(), 345); + + route_handler_->setRouteLanelets(path_lanelets); + ASSERT_TRUE(route_handler_->isHandlerReady()); + + rclcpp::init(0, nullptr); + ASSERT_TRUE(rclcpp::ok()); + + auto lanelet_sequence = route_handler_->getLaneletSequence(path_lanelets.back()); + ASSERT_EQ(lanelet_sequence.size(), 12); + ASSERT_EQ(lanelet_sequence.front().id(), 168); + ASSERT_EQ(lanelet_sequence.back().id(), 345); +} + +TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) +{ + set_route_handler("overlap_map.osm"); + set_test_route("overlap_test_route.yaml"); + ASSERT_TRUE(route_handler_->isHandlerReady()); + + geometry_msgs::msg::Pose reference_pose; + geometry_msgs::msg::Pose search_pose; + + lanelet::ConstLanelet reference_lanelet; + reference_pose.position = autoware_utils::create_point(3730.88, 73735.3, 0); + reference_pose.orientation = autoware_utils::create_quaternion(0, 0, -0.504626, 0.863338); + const auto found_reference_lanelet = + route_handler_->getClosestLaneletWithinRoute(reference_pose, &reference_lanelet); + ASSERT_TRUE(found_reference_lanelet); + ASSERT_EQ(reference_lanelet.id(), 168); + + lanelet::ConstLanelet closest_lanelet; + search_pose.position = autoware_utils::create_point(3736.89, 73730.8, 0); + search_pose.orientation = autoware_utils::create_quaternion(0, 0, 0.223244, 0.974763); + bool found_lanelet = route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lanelet); + ASSERT_TRUE(found_lanelet); + ASSERT_EQ(closest_lanelet.id(), 345); + + found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet( + search_pose, reference_lanelet, &closest_lanelet, dist_threshold, yaw_threshold); + ASSERT_TRUE(found_lanelet); + ASSERT_EQ(closest_lanelet.id(), 277); +} + +TEST_F(TestRouteHandler, CheckLaneIsInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4785); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_TRUE(is_lane_in_goal_route_section); +} + +TEST_F(TestRouteHandler, CheckLaneIsNotInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4780); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_FALSE(is_lane_in_goal_route_section); +} + +TEST_F(TestRouteHandler, checkGetLaneletSequence) +{ + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + + lanelet::ConstLanelet closest_lanelet; + const auto found_closest_lanelet = route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + ASSERT_TRUE(found_closest_lanelet); + ASSERT_EQ(closest_lanelet.id(), 4765ul); + + const auto current_lanes = route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + + ASSERT_EQ(current_lanes.size(), 6ul); + ASSERT_EQ(current_lanes.at(0).id(), 4765ul); + ASSERT_EQ(current_lanes.at(1).id(), 4770ul); + ASSERT_EQ(current_lanes.at(2).id(), 4775ul); + ASSERT_EQ(current_lanes.at(3).id(), 4424ul); + ASSERT_EQ(current_lanes.at(4).id(), 4780ul); + ASSERT_EQ(current_lanes.at(5).id(), 4785ul); +} + +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneWhenLaneChangeToRight) +{ + const auto current_lanes = get_current_lanes(); + + // The input is within expectation. + // this lane is of preferred lane type + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // this alternative lane is a subset of preferred lane route section + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); + + // The input is within expectation. + // Although Direction::NONE, the function should still return result similar to + // Direction::RIGHT. + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // Although Direction::NONE is provided, the function should behave similarly to + // Direction::RIGHT. + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); +} + +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneUsingUnexpectedResults) +{ + const auto current_lanes = get_current_lanes(); + + std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::LEFT); + ASSERT_EQ(result.size(), 0ul); + }); +} + +TEST_F(TestRouteHandler, testGetCenterLinePath) +{ + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + { + // The input is within expectation. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 50.0); + ASSERT_EQ(center_line_path.points.size(), 51); // 26 + 26 - 1(overlapped) + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 2); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4780); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(1), 4785); + } + { + // The input is in middle of range. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 14.5, 60.5); + ASSERT_EQ(center_line_path.points.size(), 48); + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4785); + } + { + // The input is broken. + // s_start is negative, and s_end is over the boundary. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, -1.0, 200.0); + ASSERT_EQ(center_line_path.points.size(), 76); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4785); + } +} +TEST_F(TestRouteHandler, DISABLED_testGetCenterLinePathWhenLanesIsNotConnected) +{ + // broken current lanes. 4424 and 4785 are not connected directly. + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + + // The input is broken. Test is disabled because it doesn't pass. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 75.0); + ASSERT_EQ(center_line_path.points.size(), 26); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4424); +} + +TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) +{ + auto get_closest_lanelet_within_route = + [&](double x, double y, double z) -> std::optional { + const auto pose = autoware::test_utils::createPose(x, y, z, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + const auto closest_lane_obtained = + route_handler_->getClosestLaneletWithinRoute(pose, &closest_lanelet); + if (!closest_lane_obtained) { + return std::nullopt; + } + return closest_lanelet.id(); + }; + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.5, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.01, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.0, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.0, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.01, 1.75, 0).value(), 4424ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.5, 1.75, 0).value(), 4424ul); +} + +TEST_F(TestRouteHandler, testGetLaneChangeTargetLanes) +{ + { + // The input is within expectation. + // There exist no lane changing lane since both 4770 and 4775 are preferred lane. + const auto current_lanes = route_handler_->getLaneletsFromIds({4770, 4775}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + ASSERT_FALSE(lane_change_lane.has_value()); + } + + { + // The input is within expectation. + // There exist lane changing lane since 4424 is subset of preferred lane 9598. + const auto current_lanes = route_handler_->getLaneletsFromIds({4775, 4424}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + EXPECT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } + + { + // The input is within expectation. + // There is a lane-changing lane. Within the maximum current lanes, there is an alternative lane + // to the preferred lane. Therefore, the lane-changing lane exists. + const auto current_lanes = get_current_lanes(); + const auto lane_change_lane = route_handler_->getLaneChangeTarget(current_lanes); + ASSERT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } +} + +TEST_F(TestRouteHandler, testGetShoulderLaneletsAtPose) +{ + set_route_handler("overlap_map.osm"); + + geometry_msgs::msg::Pose pose; + pose.position.x = 3719.5; + pose.position.y = 73765.6; + auto shoulder_lanelets = route_handler_->getShoulderLaneletsAtPose(pose); + ASSERT_FALSE(shoulder_lanelets.empty()); + ASSERT_EQ(shoulder_lanelets.front().id(), 359ul); + + pose.position.y = 73768.6; + shoulder_lanelets = route_handler_->getShoulderLaneletsAtPose(pose); + ASSERT_TRUE(shoulder_lanelets.empty()); +} +} // namespace autoware::route_handler::test diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp new file mode 100644 index 0000000000..91754db613 --- /dev/null +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -0,0 +1,109 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ROUTE_HANDLER_HPP_ +#define TEST_ROUTE_HANDLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +namespace autoware::route_handler::test +{ + +using autoware::test_utils::get_absolute_path_to_lanelet_map; +using autoware::test_utils::get_absolute_path_to_route; +using autoware_map_msgs::msg::LaneletMapBin; +class TestRouteHandler : public ::testing::Test +{ +public: + TestRouteHandler() + { + set_route_handler("2km_test.osm"); + try { + set_test_route(lane_change_right_test_route_filename); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } + } + + TestRouteHandler(const TestRouteHandler &) = delete; + TestRouteHandler(TestRouteHandler &&) = delete; + TestRouteHandler & operator=(const TestRouteHandler &) = delete; + TestRouteHandler & operator=(TestRouteHandler &&) = delete; + ~TestRouteHandler() override = default; + + void set_route_handler(const std::string & lanelet_map_filename) + { + route_handler_.reset(); + const auto lanelet2_path = + get_absolute_path_to_lanelet_map(autoware_test_utils_dir, lanelet_map_filename); + const auto map_bin_msg = + autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); + route_handler_ = std::make_shared(map_bin_msg); + } + + void set_test_route(const std::string & route_filename) + { + const auto rh_test_route = + get_absolute_path_to_route(autoware_route_handler_dir, route_filename); + if ( + const auto route_opt = + autoware::test_utils::parse>(rh_test_route)) { + route_handler_->setRoute(*route_opt); + } else { + throw std::runtime_error( + "Failed to parse YAML file: " + rh_test_route + ". The file might be corrupted."); + } + } + + lanelet::ConstLanelets get_current_lanes() + { + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + [[maybe_unused]] const auto found_closest_lanelet = + route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + return route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + } + + std::shared_ptr route_handler_; + std::string autoware_test_utils_dir{"autoware_test_utils"}; + std::string autoware_route_handler_dir{"autoware_route_handler"}; + std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"}; + + static constexpr double center_line_resolution{5.0}; + static constexpr double dist_threshold{3.0}; + static constexpr double yaw_threshold{1.045}; + static constexpr double backward_path_length{5.0}; + static constexpr double forward_path_length{300.0}; +}; +} // namespace autoware::route_handler::test + +#endif // TEST_ROUTE_HANDLER_HPP_ diff --git a/planning/autoware_route_handler/test_route/lane_change_test_route.yaml b/planning/autoware_route_handler/test_route/lane_change_test_route.yaml new file mode 100644 index 0000000000..8b4c423c62 --- /dev/null +++ b/planning/autoware_route_handler/test_route/lane_change_test_route.yaml @@ -0,0 +1,93 @@ +header: + stamp: + sec: 1715936953 + nanosec: 67206425 + frame_id: map +start_pose: + position: + x: -50.0 + y: 1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +goal_pose: + position: + x: 70.0 + y: -1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +segments: + - preferred_primitive: + id: 4765 + primitive_type: "" + primitives: + - id: 4765 + primitive_type: lane + - id: 9590 + primitive_type: lane + - preferred_primitive: + id: 4770 + primitive_type: "" + primitives: + - id: 4770 + primitive_type: lane + - id: 5072 + primitive_type: lane + - preferred_primitive: + id: 4775 + primitive_type: "" + primitives: + - id: 4775 + primitive_type: lane + - id: 9594 + primitive_type: lane + - preferred_primitive: + id: 9598 + primitive_type: "" + primitives: + - id: 4424 + primitive_type: lane + - id: 9598 + primitive_type: lane + - preferred_primitive: + id: 5084 + primitive_type: "" + primitives: + - id: 4780 + primitive_type: lane + - id: 5084 + primitive_type: lane + - preferred_primitive: + id: 5088 + primitive_type: "" + primitives: + - id: 4785 + primitive_type: lane + - id: 5088 + primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false diff --git a/planning/autoware_route_handler/test_route/overlap_test_route.yaml b/planning/autoware_route_handler/test_route/overlap_test_route.yaml new file mode 100644 index 0000000000..595d22f420 --- /dev/null +++ b/planning/autoware_route_handler/test_route/overlap_test_route.yaml @@ -0,0 +1,117 @@ +header: + stamp: + sec: 1715936953 + nanosec: 67206425 + frame_id: map +start_pose: + position: + x: 3728.8 + y: 73739.2 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.513117 + w: 0.858319 +goal_pose: + position: + x: 3729.961182 + y: 73727.328125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.234831 + w: 0.972036 +segments: + - preferred_primitive: + id: 168 + primitive_type: "" + primitives: + - id: 168 + primitive_type: lane + - preferred_primitive: + id: 277 + primitive_type: "" + primitives: + - id: 277 + primitive_type: lane + - preferred_primitive: + id: 282 + primitive_type: "" + primitives: + - id: 282 + primitive_type: lane + - preferred_primitive: + id: 287 + primitive_type: "" + primitives: + - id: 287 + primitive_type: lane + - preferred_primitive: + id: 292 + primitive_type: "" + primitives: + - id: 292 + primitive_type: lane + - preferred_primitive: + id: 297 + primitive_type: "" + primitives: + - id: 297 + primitive_type: lane + - preferred_primitive: + id: 302 + primitive_type: "" + primitives: + - id: 302 + primitive_type: lane + - preferred_primitive: + id: 307 + primitive_type: "" + primitives: + - id: 307 + primitive_type: lane + - preferred_primitive: + id: 312 + primitive_type: "" + primitives: + - id: 312 + primitive_type: lane + - preferred_primitive: + id: 317 + primitive_type: "" + primitives: + - id: 317 + primitive_type: lane + - preferred_primitive: + id: 322 + primitive_type: "" + primitives: + - id: 322 + primitive_type: lane + - preferred_primitive: + id: 345 + primitive_type: "" + primitives: + - id: 345 + primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false