utils.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <array>
#include <cstddef>
#include <cstdint>
#include <optional>
#include <utility>
#include <vector>
 
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_routing/LaneletPath.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_traffic_rules/TrafficRules.h>
#include <Eigen/Core>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <rclcpp/rclcpp.hpp>
#include <route_planning_msgs/msg/route.hpp>
#include <route_planning_msgs/msg/route_element.hpp>
 
namespace lanelet2_route_planning {
 
struct LocalRouteWindow {
  route_planning_msgs::msg::Route route;
  size_t first_global_idx = 0;
};
 
std::optional<lanelet::routing::Route> getRoute(const lanelet::routing::RoutingGraphUPtr& routing_graph,
                                                const std::vector<lanelet::ConstLanelet>& route_lanelets);
 
LocalRouteWindow extractLocalRouteWindow(const route_planning_msgs::msg::Route& full_route,
                                         size_t current_global_idx,
                                         double distance_behind,
                                         double distance_ahead);
 
size_t indexOfLineStringPointClosestToPoint(const std::vector<Eigen::Vector2d>& line_string,
                                            const Eigen::Vector2d& point,
                                            const bool consider_order = false,
                                            const bool behind = true);
 
size_t matchPointToLineString(const std::vector<Eigen::Vector2d>& line_string,
                              const Eigen::Vector2d& point,
                              const size_t idx_indication,
                              const bool consider_order = false,
                              const bool behind = true);
 
size_t considerOrderForPointMatchedToLineString(const std::vector<Eigen::Vector2d>& line_string,
                                                const Eigen::Vector2d& point,
                                                const size_t idx_closest,
                                                const bool behind);
 
bool changesLaneFromPointToPoint(const Eigen::Vector2d& point, const Eigen::Vector2d& next_point, const double sampling_distance);
 
std::vector<lanelet::ConstLanelet> adjacentLeftOrRightLanelets(const lanelet::ConstLanelet& lanelet,
                                                               const lanelet::routing::RoutingGraphUPtr& routing_graph,
                                                               bool left,
                                                               bool sort_from_left = true);
 
struct ProjectedLaneletPoints {
  Eigen::Vector2d left_bound_point;   
  Eigen::Vector2d centerline_point;   
  Eigen::Vector2d right_bound_point;  
};
 
std::vector<ProjectedLaneletPoints> projectPointToLaneletLines(
    const Eigen::Vector2d& point,
    const Eigen::Vector2d& prev_point,
    const Eigen::Vector2d& next_point,
    const std::vector<lanelet::ConstLanelet>& lanelets,
    const rclcpp::Logger& logger = rclcpp::get_logger("lanelet2_route_planning"));
 
std::optional<int> computeFollowingLaneIdxOffset(const lanelet::ConstLanelet& lanelet,
                                                 const lanelet::ConstLanelet& lanelet_of_next_point,
                                                 const lanelet::routing::RoutingGraphUPtr& routing_graph);
 
route_planning_msgs::msg::RouteElement createMinimalRouteElement(const geometry_msgs::msg::Point& position,
                                                                 const geometry_msgs::msg::Quaternion& orientation,
                                                                 double s = 0.0,
                                                                 bool will_change_suggested_lane = false,
                                                                 uint8_t speed_limit = 0);
 
struct PointSequence {
  Eigen::Vector2d prev;     
  Eigen::Vector2d current;  
  Eigen::Vector2d next;     
 
  PointSequence(const Eigen::Vector2d& prev, const Eigen::Vector2d& current, const Eigen::Vector2d& next)
      : prev(prev), current(current), next(next) {}
};
 
std::pair<Eigen::Vector2d, Eigen::Vector2d> extractDrivableSpace(const lanelet::LineStringLayer& line_string_layer,
                                                                 const PointSequence& point_sequence,
                                                                 const double max_distance);
 
bool isLineStringDrivable(const lanelet::ConstLineString3d& line_string);
 
struct ExtractRegulatoryElementsResult {
  std::vector<route_planning_msgs::msg::RegulatoryElement>
      regulatory_element_msgs;                   
  std::vector<uint8_t> regulatory_element_idcs;  
  std::vector<std::vector<uint8_t>>
      adjacent_left_regulatory_element_idcs;  
  std::vector<std::vector<uint8_t>>
      adjacent_right_regulatory_element_idcs;  
};
 
ExtractRegulatoryElementsResult extractRegulatoryElements(const lanelet::ConstLanelet& lanelet,
                                                          const std::vector<lanelet::ConstLanelet>& adjacent_left_lanelets,
                                                          const std::vector<lanelet::ConstLanelet>& adjacent_right_lanelets,
                                                          const PointSequence& point_sequence);
 
std::optional<std::array<geometry_msgs::msg::Point, 2>> regulatoryElementReferenceLine(
    const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
std::optional<std::array<geometry_msgs::msg::Point, 2>> regulatoryElementCancelLine(
    const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
std::vector<geometry_msgs::msg::Point> regulatoryElementPositions(
    const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
std::pair<uint8_t, uint8_t> regulatoryElementType(const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
uint8_t regulatoryElementSpeedLimit(const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
uint8_t laneBoundaryType(const lanelet::ConstLineString2d& line);
 
uint8_t speedLimit(const lanelet::ConstLanelet& lanelet, const bool consider_regulatory_elements = true);
 
uint8_t speedLimit(const lanelet::ConstLanelet& lanelet, const Eigen::Vector2d& point);
 
std::tuple<uint8_t, int> suggestedTurnSignal(const lanelet::ConstLanelet& lanelet, const rclcpp::Logger& logger);
 
lanelet::traffic_rules::TrafficRulesPtr getTrafficRules();
 
std::optional<lanelet::ConstLanelet> laneletAtPoint(
    const Eigen::Vector2d& point,
    const lanelet::LaneletMapConstPtr& map,
    const std::optional<lanelet::traffic_rules::TrafficRulesPtr> traffic_rules = std::nullopt);
 
lanelet::ConstLanelet followLaneletsAlongRoutingGraph(const lanelet::routing::RoutingGraphUPtr& routing_graph,
                                                      const lanelet::ConstLanelet& lanelet,
                                                      const Eigen::Vector2d& position,
                                                      const double distance);
 
struct ResampleCenterlinesAlongPathResult {
  std::vector<Eigen::Vector3d> centerline;   
  std::vector<size_t> lanelet_idx_by_point;  
};
 
ResampleCenterlinesAlongPathResult resampleCenterlinesAlongPath(const lanelet::routing::LaneletPath& path,
                                                                const double delta_s,
                                                                bool monotonically);
 
double distanceTraveled(const route_planning_msgs::msg::Route& route);
 
double distanceRemaining(const route_planning_msgs::msg::Route& route);
 
double estimateRemainingTime(const route_planning_msgs::msg::Route& route, const double reference_speed = 50.0 / 3.6);
 
void postprocessRouteMessage(
    route_planning_msgs::msg::Route& route_msg,
    std::vector<std::vector<int>>& suggested_turn_signal_distance_ahead_by_route_element_by_lane_element);
 
void sortLaneletsByMatchingCost(const lanelet::BasicPoint2d& point,
                                std::vector<std::pair<double, lanelet::ConstLanelet>>& lanelets_with_distances,
                                const std::optional<lanelet::traffic_rules::TrafficRulesPtr>& traffic_rules = std::nullopt);
 
}  // namespace lanelet2_route_planning