diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index d13c0536cf3c1..73bf99962ea82 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -205,54 +205,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, const bool get_shoulder_lane = false) const; - /** - * @brief Searches the furthest linestring to the right side of the lanelet - * Only lanelet with same direction is considered - * @param the lanelet of interest - * @return right most linestring of the lane with same direction - */ - lanelet::ConstLineString3d getRightMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Searches the furthest linestring to the right side of the lanelet - * Used to search for road shoulders. Lane direction is ignored - * @param the lanelet of interest - * @return right most linestring - */ - lanelet::ConstLineString3d getRightMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Searches the furthest linestring to the left side of the lanelet - * Only lanelet with same direction is considered - * @param the lanelet of interest - * @return left most linestring of the lane with same direction - */ - lanelet::ConstLineString3d getLeftMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Searches the furthest linestring to the left side of the lanelet - * Used to search for road shoulders. Lane direction is ignored - * @param the lanelet of interest - * @return left most linestring - */ - lanelet::ConstLineString3d getLeftMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Return furthest linestring on both side of the lanelet - * @param the lanelet of interest - * @param (optional) search furthest right side - * @param (optional) search furthest left side - * @param (optional) include opposite lane as well - * @return right and left linestrings - */ - lanelet::ConstLineStrings3d getFurthestLinestring( - const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true, - bool is_opposite = true, bool enable_same_root = false) const noexcept; - /** * Retrieves a sequence of lanelets before the given lanelet. * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence @@ -276,18 +228,6 @@ class RouteHandler 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. - * The total distance is computed from the front point of the centerline of the given lane to - * the front point of the preferred lane. - * @param lanelet lanelet to query - * @param direction change direction - * @return number of lanes from input to the preferred lane - */ - double getTotalLateralDistanceToPreferredLane( - 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. @@ -321,11 +261,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, const Pose & pose, const double backward_distance = std::numeric_limits::max(), const double forward_distance = std::numeric_limits::max()) const; - lanelet::ConstLanelets getCheckTargetLanesFromPath( - const PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes, - const double check_length) const; - lanelet::routing::RelationType getRelation( - const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const; bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const; bool isRoadLanelet(const lanelet::ConstLanelet & lanelet) const; @@ -339,14 +274,9 @@ class RouteHandler const lanelet::ConstLanelets & lanelets, const Direction direction = Direction::NONE) const; std::optional getLaneChangeTargetExceptPreferredLane( const lanelet::ConstLanelets & lanelets, const Direction direction) const; - bool getRightLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; - bool getLeftLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; std::optional getPullOverTarget(const Pose & goal_pose) const; std::optional getPullOutStartLane( const Pose & pose, const double vehicle_width) const; - double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const; lanelet::ConstLanelets getRoadLaneletsAtPose(const Pose & pose) const; std::optional getLeftShoulderLanelet( const lanelet::ConstLanelet & lanelet) const; @@ -354,7 +284,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; - bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const; private: // MUST @@ -385,8 +314,6 @@ class RouteHandler lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const; // for lanelet - bool isInTargetLane(const PoseStamped & pose, const lanelet::ConstLanelets & target) const; - bool isInPreferredLane(const PoseStamped & pose) const; bool isBijectiveConnection( const lanelet::ConstLanelets & lanelet_section1, const lanelet::ConstLanelets & lanelet_section2) const; @@ -419,17 +346,13 @@ class RouteHandler const double min_length = std::numeric_limits::max()) const; lanelet::ConstLanelets getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const; - lanelet::ConstLanelets getLaneChangeTargetLanes(const Pose & pose) const; lanelet::ConstLanelets getLaneSequenceUpTo(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneSequenceAfter(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneSequence(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getNeighborsWithinRoute(const lanelet::ConstLanelet & lanelet) const; - std::vector getLaneSection(const lanelet::ConstLanelet & lanelet) const; - lanelet::ConstLanelets getNextLaneSequence(const lanelet::ConstLanelets & lane_sequence) const; // for path - PathWithLaneId updatePathTwist(const PathWithLaneId & path) const; /** * @brief Checks if a path has a no_drivable_lane or not * @param path lanelet path diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 589a98d7d5ab4..e9fb3cc7d2fd7 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1324,98 +1324,6 @@ lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( return lanelet; } -lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); - - if (same) { - return getRightMostSameDirectionLinestring(same.value(), enable_same_root); - } - - return lanelet.rightBound(); -} - -lanelet::ConstLineString3d RouteHandler::getRightMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - const auto & same = getRightLanelet(lanelet, enable_same_root); - const auto & opposite = getRightOppositeLanelets(lanelet); - if (!same && opposite.empty()) { - return lanelet.rightBound(); - } - - if (same) { - return getRightMostLinestring(same.value(), enable_same_root); - } - - if (!opposite.empty()) { - return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front()), false); - } - - return lanelet.rightBound(); -} - -lanelet::ConstLineString3d RouteHandler::getLeftMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); - - if (same) { - return getLeftMostSameDirectionLinestring(same.value(), enable_same_root); - } - - return lanelet.leftBound(); -} - -lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); - const auto & opposite = getLeftOppositeLanelets(lanelet); - if (!same && opposite.empty()) { - return lanelet.leftBound(); - } - - if (same) { - return getLeftMostLinestring(same.value(), enable_same_root); - } - - if (!opposite.empty()) { - return getRightMostLinestring(lanelet::ConstLanelet(opposite.front()), false); - } - - return lanelet.leftBound(); -} - -lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring( - const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, bool is_opposite, - bool enable_same_root) const noexcept -{ - lanelet::ConstLineStrings3d linestrings; - linestrings.reserve(2); - - if (is_right && is_opposite) { - linestrings.emplace_back(getRightMostLinestring(lanelet, enable_same_root)); - } else if (is_right && !is_opposite) { - linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet, enable_same_root)); - } else { - linestrings.emplace_back(lanelet.rightBound()); - } - - if (is_left && is_opposite) { - linestrings.emplace_back(getLeftMostLinestring(lanelet, enable_same_root)); - } else if (is_left && !is_opposite) { - linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet, enable_same_root)); - } else { - linestrings.emplace_back(lanelet.leftBound()); - } - return linestrings; -} - std::vector RouteHandler::getPrecedingLaneletSequence( const lanelet::ConstLanelet & lanelet, const double length, const lanelet::ConstLanelets & exclude_lanelets) const @@ -1486,46 +1394,6 @@ std::optional RouteHandler::getLaneChangeTargetExceptPref return std::nullopt; } -bool RouteHandler::getRightLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const -{ - for (const auto & lanelet : lanelets) { - const int num = getNumLaneToPreferredLane(lanelet); - - // Get right lanelet if preferred lane is on the left - if (num >= 0) { - if (!!routing_graph_ptr_->right(lanelet)) { - const auto right_lanelet = routing_graph_ptr_->right(lanelet); - *target_lanelet = right_lanelet.value(); - return true; - } - } - } - - *target_lanelet = lanelets.front(); - return false; -} - -bool RouteHandler::getLeftLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const -{ - for (const auto & lanelet : lanelets) { - const int num = getNumLaneToPreferredLane(lanelet); - - // Get left lanelet if preferred lane is on the right - if (num <= 0) { - if (!!routing_graph_ptr_->left(lanelet)) { - const auto left_lanelet = routing_graph_ptr_->left(lanelet); - *target_lanelet = left_lanelet.value(); - return true; - } - } - } - - *target_lanelet = lanelets.front(); - return false; -} - std::optional RouteHandler::getPullOverTarget(const Pose & goal_pose) const { const lanelet::BasicPoint2d p(goal_pose.position.x, goal_pose.position.y); @@ -1589,13 +1457,6 @@ int RouteHandler::getNumLaneToPreferredLane( return 0; // TODO(Horibe) check if return 0 is appropriate. } -double RouteHandler::getTotalLateralDistanceToPreferredLane( - const lanelet::ConstLanelet & lanelet, const Direction direction) const -{ - const auto intervals = getLateralIntervalsToPreferredLane(lanelet, direction); - return std::accumulate(intervals.begin(), intervals.end(), 0); -} - std::vector RouteHandler::getLateralIntervalsToPreferredLane( const lanelet::ConstLanelet & lanelet, const Direction direction) const { @@ -1650,30 +1511,6 @@ std::vector RouteHandler::getLateralIntervalsToPreferredLane( return {}; } -bool RouteHandler::isPreferredLane(const lanelet::ConstLanelet & lanelet) const -{ - return exists(preferred_lanelets_, lanelet); -} - -bool RouteHandler::isInPreferredLane(const PoseStamped & pose) const -{ - lanelet::ConstLanelet lanelet; - if (!getClosestLaneletWithinRoute(pose.pose, &lanelet)) { - return false; - } - return exists(preferred_lanelets_, lanelet); -} - -bool RouteHandler::isInTargetLane( - const PoseStamped & pose, const lanelet::ConstLanelets & target) const -{ - lanelet::ConstLanelet lanelet; - if (!getClosestLaneletWithinRoute(pose.pose, &lanelet)) { - return false; - } - return exists(target, lanelet); -} - PathWithLaneId RouteHandler::getCenterLinePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact) const @@ -1749,139 +1586,6 @@ PathWithLaneId RouteHandler::getCenterLinePath( return reference_path; } -PathWithLaneId RouteHandler::updatePathTwist(const PathWithLaneId & path) const -{ - PathWithLaneId updated_path = path; - for (auto & point : updated_path.points) { - const auto id = point.lane_ids.at(0); - const auto llt = lanelet_map_ptr_->laneletLayer.get(id); - const auto limit = traffic_rules_ptr_->speedLimit(llt); - point.point.longitudinal_velocity_mps = static_cast(limit.speedLimit.value()); - } - return updated_path; -} - -lanelet::ConstLanelets RouteHandler::getLaneChangeTargetLanes(const Pose & pose) const -{ - lanelet::ConstLanelet lanelet; - lanelet::ConstLanelets target_lanelets; - if (!getClosestLaneletWithinRoute(pose, &lanelet)) { - return target_lanelets; - } - - const int num = getNumLaneToPreferredLane(lanelet); - if (num < 0) { - const auto right_lanelet = (!!routing_graph_ptr_->right(lanelet)) - ? routing_graph_ptr_->right(lanelet) - : routing_graph_ptr_->adjacentRight(lanelet); - target_lanelets = getLaneletSequence(right_lanelet.value()); - } - if (num > 0) { - const auto left_lanelet = (!!routing_graph_ptr_->left(lanelet)) - ? routing_graph_ptr_->left(lanelet) - : routing_graph_ptr_->adjacentLeft(lanelet); - target_lanelets = getLaneletSequence(left_lanelet.value()); - } - return target_lanelets; -} - -double RouteHandler::getLaneChangeableDistance( - const Pose & current_pose, const Direction & direction) const -{ - lanelet::ConstLanelet current_lane; - if (!getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - return 0; - } - - // get lanelets after current lane - auto lanelet_sequence = getLaneletSequenceAfter(current_lane); - lanelet_sequence.insert(lanelet_sequence.begin(), current_lane); - - double accumulated_distance = 0; - for (const auto & lane : lanelet_sequence) { - lanelet::ConstLanelet target_lane; - if (direction == Direction::RIGHT) { - if (!getRightLaneletWithinRoute(lane, &target_lane)) { - break; - } - } - if (direction == Direction::LEFT) { - if (!getLeftLaneletWithinRoute(lane, &target_lane)) { - break; - } - } - double lane_length = lanelet::utils::getLaneletLength3d(lane); - - // overwrite goal because lane change must be finished before reaching goal - if (isInGoalRouteSection(lane)) { - const auto goal_position = lanelet::utils::conversion::toLaneletPoint(getGoalPose().position); - const auto goal_arc_coordinates = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(lane.centerline()), lanelet::utils::to2D(goal_position).basicPoint()); - lane_length = std::min(goal_arc_coordinates.length, lane_length); - } - - // subtract distance up to current position for first lane - if (lane == current_lane) { - const auto current_position = - lanelet::utils::conversion::toLaneletPoint(current_pose.position); - const auto arc_coordinate = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(lane.centerline()), - lanelet::utils::to2D(current_position).basicPoint()); - lane_length = std::max(lane_length - arc_coordinate.length, 0.0); - } - accumulated_distance += lane_length; - } - return accumulated_distance; -} - -lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( - const PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes, - const double check_length) const -{ - std::vector target_lane_ids; - target_lane_ids.reserve(target_lanes.size()); - for (const auto & llt : target_lanes) { - target_lane_ids.push_back(llt.id()); - } - - // find first lanelet in target lanes along path - int64_t root_lane_id = lanelet::InvalId; - for (const auto & point : path.points) { - for (const auto & lane_id : point.lane_ids) { - if (exists(target_lane_ids, lane_id)) { - root_lane_id = lane_id; - } - } - } - // return emtpy lane if failed to find root lane_id - if (root_lane_id == lanelet::InvalId) { - return target_lanes; - } - lanelet::ConstLanelet root_lanelet; - for (const auto & llt : target_lanes) { - if (llt.id() == root_lane_id) { - root_lanelet = llt; - } - } - - const auto sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr_, root_lanelet, check_length); - lanelet::ConstLanelets check_lanelets; - for (const auto & sequence : sequences) { - for (const auto & llt : sequence) { - if (!lanelet::utils::contains(check_lanelets, llt)) { - check_lanelets.push_back(llt); - } - } - } - for (const auto & llt : target_lanes) { - if (!lanelet::utils::contains(check_lanelets, llt)) { - check_lanelets.push_back(llt); - } - } - return check_lanelets; -} - bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; @@ -1908,41 +1612,6 @@ lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const return lanelet_map_ptr_; } -lanelet::routing::RelationType RouteHandler::getRelation( - const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const -{ - if (prev_lane == next_lane) { - return lanelet::routing::RelationType::None; - } - const auto & relation = routing_graph_ptr_->routingRelation(prev_lane, next_lane); - if (relation) { - return relation.value(); - } - - // check if lane change extends across multiple lanes - const auto shortest_path = routing_graph_ptr_->shortestPath(prev_lane, next_lane); - if (shortest_path) { - auto prev_llt = shortest_path->front(); - for (const auto & llt : shortest_path.value()) { - if (prev_llt == llt) { - continue; - } - const auto & relation = routing_graph_ptr_->routingRelation(prev_llt, llt); - if (!relation) { - continue; - } - if ( - relation.value() == lanelet::routing::RelationType::Left || - relation.value() == lanelet::routing::RelationType::Right) { - return relation.value(); - } - prev_llt = llt; - } - } - - return lanelet::routing::RelationType::None; -} - bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const { return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && @@ -2100,33 +1769,6 @@ lanelet::ConstLanelets RouteHandler::getNeighborsWithinRoute( return neighbors_within_route; } -std::vector RouteHandler::getLaneSection( - const lanelet::ConstLanelet & lanelet) const -{ - const lanelet::ConstLanelets neighbors = getNeighborsWithinRoute(lanelet); - std::vector lane_section; - lane_section.reserve(neighbors.size()); - for (const auto & llt : neighbors) { - lane_section.push_back(getLaneSequence(llt)); - } - return lane_section; -} - -lanelet::ConstLanelets RouteHandler::getNextLaneSequence( - const lanelet::ConstLanelets & lane_sequence) const -{ - lanelet::ConstLanelets next_lane_sequence; - if (lane_sequence.empty()) { - return next_lane_sequence; - } - const auto & final_lanelet = lane_sequence.back(); - lanelet::ConstLanelet next_lanelet; - if (!getNextLaneletWithinRoute(final_lanelet, &next_lanelet)) { - return next_lane_sequence; - } - return getLaneSequence(next_lanelet); -} - bool RouteHandler::planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const