lanelet2_route_planning Namespace Reference

Classes
struct	ExtractRegulatoryElementsResult
	Return type of extractRegulatoryElements. More...
struct	IntersectionOfLinesResult
	Return type of intersectionOfLines. More...
struct	is_vector
struct	is_vector< std::vector< T, A > >
class	Lanelet2RoutePlanning
	Lanelet2 route planning node. More...
struct	LocalRouteWindow
	A local route window and its offset in the complete route. More...
struct	PointSequence
	Helper type for a sequence of three points. More...
struct	ProjectedLaneletPoints
	Projected lanelet points. More...
struct	ProjectPointToLineStringAlongAxisResult
	Return type of projectPointToLineStringAlongAxis. More...
struct	ResampleCenterlinesAlongPathResult
	Return type of resampleCenterlinesAlongPath. More...
struct	TopicDiagnosticConfig
	Configuration parameters for topic diagnostics. More...

Functions
Eigen::Vector2d	to2d (const Eigen::Vector3d &point)
	Converts a 3D Eigen point to a 2D Eigen point.
Eigen::Vector3d	to3d (const Eigen::Vector2d &point)
	Converts a 2D Eigen point to a 3D Eigen point.
Eigen::Vector3d	to3d (const Eigen::Vector2d &point, double z)
	Converts a 2D Eigen point to a 3D Eigen point.
std::vector< Eigen::Vector2d >	to2d (const std::vector< Eigen::Vector3d > &points)
	Converts a vector of 3D Eigen points to a vector of 2D Eigen points.
std::vector< Eigen::Vector3d >	to3d (const std::vector< Eigen::Vector2d > &points)
	Converts a vector of 2D Eigen points to a vector of 3D Eigen points.
geometry_msgs::msg::Point	toRos (const Eigen::Vector2d &point)
	Converts a 2D Eigen point to a ROS point.
geometry_msgs::msg::Point	toRos (const Eigen::Vector3d &point)
	Converts a 3D Eigen point to a ROS point.
geometry_msgs::msg::Point	toRos (const lanelet::BasicPoint2d &point)
	Converts a 2D Lanelet point to a ROS point.
lanelet::BasicPoint2d	toLanelet (const Eigen::Vector2d &point)
	Converts a 2D Eigen point to a Lanelet point.
lanelet::BasicPoint3d	toLanelet (const Eigen::Vector3d &point)
	Converts a 3D Eigen point to a Lanelet point.
Eigen::Vector2d	toEigen2d (const geometry_msgs::msg::Point &point)
	Converts a ROS point to a 2D Eigen point.
Eigen::Vector3d	toEigen (const geometry_msgs::msg::Point &point)
	Converts a ROS point to a 3D Eigen point.
geometry_msgs::msg::Point	egoPosition (const perception_msgs::msg::EgoData &ego_data)
	Extracts an EgoData position as a ROS point.
std::vector< Eigen::Vector2d >	toEigen (const lanelet::BasicLineString2d &line_string)
	Converts a 2D Lanelet line string to a vector of Eigen points.
std::vector< Eigen::Vector3d >	toEigen (const lanelet::BasicLineString3d &line_string)
	Converts a 3D Lanelet line string to a vector of Eigen points.
lanelet::BasicLineString2d	toLanelet (const std::vector< Eigen::Vector2d > &line_string)
	Converts a vector of 2D Eigen points to a Lanelet line string.
lanelet::BasicLineString3d	toLanelet (const std::vector< Eigen::Vector3d > &line_string)
	Converts a vector of 3D Eigen points to a Lanelet line string.
geometry_msgs::msg::Quaternion	toRosQuaternion (const Eigen::Vector2d &vector)
	Converts a 2D Eigen vector pointing in a specific direction to a ROS quaternion.
std::vector< Eigen::Vector3d >	suggestedReferenceLineToEigen (const std::vector< route_planning_msgs::msg::RouteElement > &route_elements)
	Extracts the reference line along the suggested lane from a list of RouteElements.
double	wrapAngle (const double angle)
	Wraps an angle to the range [-π, π].
double	angleBetweenVectors (const Eigen::Vector2d &v1, const Eigen::Vector2d &v2)
	Computes the angle between two 2D vectors.
std::optional< IntersectionOfLinesResult >	intersectionOfLines (const std::vector< Eigen::Vector2d > &line1, const std::vector< Eigen::Vector2d > &line2)
	Computes the intersection of two 2D lines.
Eigen::Vector2d	tangentOfPointAlongLineString (const Eigen::Vector2d &point, const Eigen::Vector2d &prev_point, const Eigen::Vector2d &next_point)
	Computes a unit vector tangential to a point along a line string.
Eigen::Vector2d	normalOfPointAlongLineString (const Eigen::Vector2d &point, const Eigen::Vector2d &prev_point, const Eigen::Vector2d &next_point)
	Computes a unit vector normal to a point along a line string.
std::vector< Eigen::Vector3d >	resampleLineString (const std::vector< Eigen::Vector3d > &line_string, const double delta, double &offset)
	Resamples a 3D line string with a constant sampling distance.
std::vector< size_t >	adaptivelySampleLineString (const std::vector< Eigen::Vector2d > &line_string, double max_lateral_error, const std::vector< size_t > &break_after_indices={})
	Simplifies a 2D line string within a maximum lateral error.
Eigen::Vector2d	projectPointToLineString (const Eigen::Vector2d &point, const std::vector< Eigen::Vector2d > &line_string)
	Projects a point to the closest line segment of a line string.
Eigen::Vector3d	projectPointToLineString (const Eigen::Vector3d &point, const std::vector< Eigen::Vector3d > &line_string)
	Projects a 3D point to the closest line segment of a 3D line string.
std::optional< ProjectPointToLineStringAlongAxisResult >	projectPointToLineStringAlongAxis (const Eigen::Vector2d &point, const Eigen::Vector2d &axis, const std::vector< Eigen::Vector2d > &line_string)
	Projects a point along an axis to the closest line segment of a line string.
std::optional< ProjectPointToLineStringAlongAxisResult >	projectPointToLineStringAlongNormal (const Eigen::Vector2d &point, const Eigen::Vector2d &prev_point, const Eigen::Vector2d &next_point, const std::vector< Eigen::Vector2d > &line_string)
	Projects a point to the closest line segment of a line string along the normal to the tangent at the point.
std::optional< lanelet::routing::Route >	getRoute (const lanelet::routing::RoutingGraphUPtr &routing_graph, const std::vector< lanelet::ConstLanelet > &route_lanelets)
	Computes a route from start to destination along intermediate destinations.
LocalRouteWindow	extractLocalRouteWindow (const route_planning_msgs::msg::Route &full_route, size_t current_global_idx, double distance_behind, double distance_ahead)
	Extracts a local route window while preserving absolute route data.
size_t	indexOfLineStringPointClosestToPoint (const std::vector< Eigen::Vector2d > &line_string, const Eigen::Vector2d &point, const bool consider_order=false, const bool behind=true)
	Finds the index of a point in a line string that is closest to another point.
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)
	Finds the index of a point in a line string that is locally closest to another point.
size_t	considerOrderForPointMatchedToLineString (const std::vector< Eigen::Vector2d > &line_string, const Eigen::Vector2d &point, const size_t idx_closest, const bool behind)
	Takes a closest point in a line string and guarantees that it is behind or ahead of the given point.
bool	changesLaneFromPointToPoint (const Eigen::Vector2d &point, const Eigen::Vector2d &next_point, const double sampling_distance)
	Identifies a lane change based on the distance between two reference line points.
std::vector< lanelet::ConstLanelet >	adjacentLeftOrRightLanelets (const lanelet::ConstLanelet &lanelet, const lanelet::routing::RoutingGraphUPtr &routing_graph, bool left, bool sort_from_left=true)
	Finds lanelets adjacent to the left or right of a given lanelet.
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"))
	Projects a point to the centerline and bounds of a set of lanelets.
std::optional< int >	computeFollowingLaneIdxOffset (const lanelet::ConstLanelet &lanelet, const lanelet::ConstLanelet &lanelet_of_next_point, const lanelet::routing::RoutingGraphUPtr &routing_graph)
	Computes the offset of lane element indices from current to next route element.
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)
	Create a minimal route element message.
std::pair< Eigen::Vector2d, Eigen::Vector2d >	extractDrivableSpace (const lanelet::LineStringLayer &line_string_layer, const PointSequence &point_sequence, const double max_distance)
	Extracts drivable space boundaries for a route element.
bool	isLineStringDrivable (const lanelet::ConstLineString3d &line_string)
	Checks if lanelet line string has a type that is considered drivable.
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)
	Extracts regulatory element information for a route element.
std::optional< std::array< geometry_msgs::msg::Point, 2 > >	regulatoryElementReferenceLine (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the reference/effect line of a regulatory element.
std::optional< std::array< geometry_msgs::msg::Point, 2 > >	regulatoryElementCancelLine (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the cancel line of a regulatory element.
std::vector< geometry_msgs::msg::Point >	regulatoryElementPositions (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the sign/signal positions of a regulatory element.
std::pair< uint8_t, uint8_t >	regulatoryElementType (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the type and meta value of a regulatory element.
uint8_t	regulatoryElementSpeedLimit (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the speed limit of a regulatory element of subtype 'speed_limit'.
uint8_t	laneBoundaryType (const lanelet::ConstLineString2d &line)
	Extracts the lane boundary type of a lanelet line.
uint8_t	speedLimit (const lanelet::ConstLanelet &lanelet, const bool consider_regulatory_elements=true)
	Extracts the speed limit of a lanelet.
uint8_t	speedLimit (const lanelet::ConstLanelet &lanelet, const Eigen::Vector2d &point)
	Extracts the speed limit of a lanelet at a specific position on that lanelet.
std::tuple< uint8_t, int >	suggestedTurnSignal (const lanelet::ConstLanelet &lanelet, const rclcpp::Logger &logger)
	Extracts the suggested turn signal of a lanelet.
lanelet::traffic_rules::TrafficRulesPtr	getTrafficRules ()
	Get traffic rules.
std::optional< lanelet::ConstLanelet >	laneletAtPoint (const Eigen::Vector2d &point, const lanelet::LaneletMapConstPtr &map, const std::optional< lanelet::traffic_rules::TrafficRulesPtr > traffic_rules=std::nullopt)
	Find lanelet at arbitrary point.
lanelet::ConstLanelet	followLaneletsAlongRoutingGraph (const lanelet::routing::RoutingGraphUPtr &routing_graph, const lanelet::ConstLanelet &lanelet, const Eigen::Vector2d &position, const double distance)
	Follows a lanelet's and following lanelets' centerline for a given distance.
ResampleCenterlinesAlongPathResult	resampleCenterlinesAlongPath (const lanelet::routing::LaneletPath &path, const double delta_s, bool monotonically)
	Equidistantly resamples lanelet centerlines along a path to one joint centerline.
double	distanceTraveled (const route_planning_msgs::msg::Route &route)
	Computes the traveled distance along the route.
double	distanceRemaining (const route_planning_msgs::msg::Route &route)
	Computes the remaining distance along the route.
double	estimateRemainingTime (const route_planning_msgs::msg::Route &route, const double reference_speed=50.0/3.6)
	Estimate remaining time for a route based on speed limits.
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)
	Postprocesses a route message, filling missing information that can be inferred from other message contents.
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)
	Sorts lanelet candidates by their matching cost for a 2D point.

Variables
template<typename C >
constexpr bool	is_vector_v = is_vector<C>::value

Function Documentation

adaptivelySampleLineString()

std::vector< size_t > lanelet2_route_planning::adaptivelySampleLineString	(	const std::vector< Eigen::Vector2d > &	line_string,
		double	max_lateral_error,
		const std::vector< size_t > &	break_after_indices = {} )

Simplifies a 2D line string within a maximum lateral error.

Uses Ramer-Douglas-Peucker simplification and returns indices into the input. Breaks split the line after the given indices so discontinuities are retained. A maximum error of zero disables simplification.

Parameters

[in]	line_string	line string
[in]	max_lateral_error	maximum perpendicular deviation [m]
[in]	break_after_indices	indices after which to split the line

Returns

retained input indices in ascending order

Definition at line 188 of file geometry.cpp.

                                                                                             {
  std::vector<size_t> retained_indices;
  if (line_string.empty()) {
    return retained_indices;
  }
  if (max_lateral_error <= 0.0) {
    retained_indices.resize(line_string.size());
    std::iota(retained_indices.begin(), retained_indices.end(), 0);
    return retained_indices;
  }
 
  // retain discontinuities, such as lane changes, as separate ranges
  std::vector<size_t> valid_breaks;
  for (const size_t index : break_after_indices) {
    if (index < line_string.size() - 1) {
      valid_breaks.push_back(index);
    }
  }
  std::sort(valid_breaks.begin(), valid_breaks.end());
  valid_breaks.erase(std::unique(valid_breaks.begin(), valid_breaks.end()), valid_breaks.end());
 
  // sample each continuous range independently
  std::vector<bool> keep(line_string.size(), false);
  size_t segment_start = 0;
  for (const size_t break_after : valid_breaks) {
    if (break_after < segment_start) {
      continue;
    }
    adaptivelySampleLineStringRange(line_string, segment_start, break_after, max_lateral_error, keep);
    segment_start = break_after + 1;
  }
  adaptivelySampleLineStringRange(line_string, segment_start, line_string.size() - 1, max_lateral_error, keep);
 
  for (size_t i = 0; i < keep.size(); ++i) {
    if (keep[i]) {
      retained_indices.push_back(i);
    }
  }
  return retained_indices;
}

adjacentLeftOrRightLanelets()

std::vector< lanelet::ConstLanelet > lanelet2_route_planning::adjacentLeftOrRightLanelets	(	const lanelet::ConstLanelet &	lanelet,
		const lanelet::routing::RoutingGraphUPtr &	routing_graph,
		bool	left,
		bool	sort_from_left = true )

Finds lanelets adjacent to the left or right of a given lanelet.

RelationType::Left and RelationType::AdjacentLeft are used for left adjacent lanelets, right vice versa.

Parameters

[in]	lanelet	lanelet
[in]	routing_graph	routing graph
[in]	left	whether to find left or right adjacent lanelets
[in]	sort_from_left	whether to sort from left to right (true) or right to left (false)

Returns

adjacent lanelets

Definition at line 184 of file utils.cpp.

                                                                                    {
  std::vector<lanelet::ConstLanelet> adjacent_lanelets;
  const int routing_cost_id = 0;  // RoutingCostDistance
  lanelet::routing::LaneletRelations relations =
      left ? routing_graph->leftRelations(lanelet, routing_cost_id) : routing_graph->rightRelations(lanelet, routing_cost_id);
  for (const auto& relation : relations) {
    if ((left && (relation.relationType == lanelet::routing::RelationType::Left ||
                  relation.relationType == lanelet::routing::RelationType::AdjacentLeft)) ||
        (!left && (relation.relationType == lanelet::routing::RelationType::Right ||
                   relation.relationType == lanelet::routing::RelationType::AdjacentRight))) {
      adjacent_lanelets.push_back(relation.lanelet);
    }
  }
 
  if ((left && sort_from_left) || (!left && !sort_from_left)) {
    std::reverse(adjacent_lanelets.begin(), adjacent_lanelets.end());
  }
 
  return adjacent_lanelets;
}

angleBetweenVectors()

double lanelet2_route_planning::angleBetweenVectors	(	const Eigen::Vector2d &	v1,
		const Eigen::Vector2d &	v2 )

Computes the angle between two 2D vectors.

Angle is in the range [-π, π].

Parameters

[in]	v1	vector 1
[in]	v2	vector 2

Returns

angle between vectors [radians]

Definition at line 80 of file geometry.cpp.

                                                                             {
  double dot_product = v1.dot(v2);
  double determinant = v1.x() * v2.y() - v1.y() * v2.x();
  double angle = std::atan2(determinant, dot_product);
  return wrapAngle(angle);
}

changesLaneFromPointToPoint()

bool lanelet2_route_planning::changesLaneFromPointToPoint	(	const Eigen::Vector2d &	point,
		const Eigen::Vector2d &	next_point,
		const double	sampling_distance )

Identifies a lane change based on the distance between two reference line points.

Assumes that the two points are part of an equidistant reference line. If the distance between the two points is greater than the sampling distance, a lane change is assumed.

Parameters

[in]	point	point on reference line
[in]	next_point	next point on reference line
[in]	sampling_distance	expected distance between points

Returns

whether a lane change is identified

Definition at line 177 of file utils.cpp.

                                                                 {
  const double epsilon = 1e-6;
  return ((next_point - point).norm() > (sampling_distance + epsilon));
}

computeFollowingLaneIdxOffset()

std::optional< int > lanelet2_route_planning::computeFollowingLaneIdxOffset	(	const lanelet::ConstLanelet &	lanelet,
		const lanelet::ConstLanelet &	lanelet_of_next_point,
		const lanelet::routing::RoutingGraphUPtr &	routing_graph )

Computes the offset of lane element indices from current to next route element.

The algorithm works as follows:

• find adjacent lanelets of current lanelet
• find adjacent lanelets of next lanelet (lanelet of next point)
• try to match the ID of the lanelet following the current one to the next lanelet and its adjacent lanelets
• if no match is found, try to match the ID of the lanelets following adjacent lanelets of the current lanelet to the next lanelet and its adjacent lanelets
• keep following lanelets for up to 3 iterations to account for lanelets that are skipped during sampling

If lane element j of the next route element follows on lane element i of the current route element, the returned offset is (j-i).

Parameters

[in]	lanelet	current lanelet
[in]	lanelet_of_next_point	lanelet of next point (on next route element)
[in]	routing_graph	routing graph

Returns

following lane index offset

Definition at line 252 of file utils.cpp.

                                                                                                      {
  int following_lane_idx_offset = 0;
  if (lanelet_of_next_point.id() != lanelet.id()) {
    // get adjacent lanelets of current lanelet
    std::vector<lanelet::ConstLanelet> adjacent_left_lanelets = adjacentLeftOrRightLanelets(lanelet, routing_graph, true);
    std::vector<lanelet::ConstLanelet> adjacent_right_lanelets = adjacentLeftOrRightLanelets(lanelet, routing_graph, false);
    const int suggested_lane_idx = static_cast<int>(adjacent_left_lanelets.size());
 
    // get adjacent lanelets of next lanelet (lanelet of next point)
    std::vector<lanelet::ConstLanelet> adjacent_left_lanelets_of_next_lanelet =
        adjacentLeftOrRightLanelets(lanelet_of_next_point, routing_graph, true);
    std::vector<lanelet::ConstLanelet> adjacent_right_lanelets_of_next_lanelet =
        adjacentLeftOrRightLanelets(lanelet_of_next_point, routing_graph, false);
    std::vector<lanelet::ConstLanelet> adjacent_lanelets_of_next_lanelet = adjacent_left_lanelets_of_next_lanelet;
    adjacent_lanelets_of_next_lanelet.push_back(lanelet_of_next_point);
    adjacent_lanelets_of_next_lanelet.insert(adjacent_lanelets_of_next_lanelet.end(),
                                             adjacent_right_lanelets_of_next_lanelet.begin(),
                                             adjacent_right_lanelets_of_next_lanelet.end());
 
    // find following lanelet of current lanelet and adjacent lanelets in adjacent lanelets of next lanelet
    std::vector<std::vector<lanelet::ConstLanelet>> lanelet_groups = {{lanelet}, adjacent_left_lanelets, adjacent_right_lanelets};
    std::vector<int> lanelet_group_offset_factors = {0, 1, -1};
    following_lane_idx_offset = std::numeric_limits<int>::max();
 
    // first try to match following lanelet of current lanelet, then of adjacent left lanelets, then of adjacent right lanelets
    for (size_t group_idx = 0; group_idx < lanelet_groups.size(); ++group_idx) {
      const auto& lanelet_group = lanelet_groups[group_idx];
      int group_offset_factor = lanelet_group_offset_factors[group_idx];
 
      // loop over all lanelets in group
      for (size_t a = 0; a < lanelet_group.size(); ++a) {
        auto following_lanelets = routing_graph->following(lanelet_group[a], false);
        if (following_lanelets.empty()) {
          continue;
        }
        auto following_lanelet = following_lanelets.front();
        size_t following_lanelet_idx = 0;
        size_t follow_further_idx =
            0;  // counter for following lanelets more than once (e.g., if sampling skipped short lanelets)
        size_t max_follow_further_iterations = 3;  // maximum number of following lanelets to check
 
        // loop until following lanelet is found in adjacent lanelets of next lanelet
        while (following_lane_idx_offset == std::numeric_limits<int>::max()) {
          // check all adjacent lanelets of next lanelet
          for (size_t l = 0; l < adjacent_lanelets_of_next_lanelet.size(); ++l) {
            if (following_lanelet.id() == adjacent_lanelets_of_next_lanelet[l].id()) {
              following_lane_idx_offset =
                  static_cast<int>(l) - suggested_lane_idx + group_offset_factor * static_cast<int>(a + 1);
              break;
            }
          }
 
          // check abort conditions
          follow_further_idx++;
          if (follow_further_idx >= max_follow_further_iterations) {
            break;
          }
 
          // get next following lanelet
          following_lanelet_idx++;
          if (following_lanelet_idx < following_lanelets.size()) {
            following_lanelet = following_lanelets[following_lanelet_idx];
          } else {
            following_lanelets = routing_graph->following(following_lanelet, false);
            if (following_lanelets.empty()) {
              continue;
            }
            following_lanelet = following_lanelets.front();
            following_lanelet_idx = 0;
          }
        }
 
        if (following_lane_idx_offset != std::numeric_limits<int>::max()) {
          break;
        }
      }
 
      if (following_lane_idx_offset != std::numeric_limits<int>::max()) {
        break;
      }
    }
 
    if (following_lane_idx_offset == std::numeric_limits<int>::max()) {
      // could not match following lanelets to adjacent lanelets of lanelet of next point
      return std::nullopt;
    }
  }
 
  return following_lane_idx_offset;
}

considerOrderForPointMatchedToLineString()

size_t lanelet2_route_planning::considerOrderForPointMatchedToLineString	(	const std::vector< Eigen::Vector2d > &	line_string,
		const Eigen::Vector2d &	point,
		const size_t	idx_closest,
		const bool	behind )

Takes a closest point in a line string and guarantees that it is behind or ahead of the given point.

Behind/ahead is meant physically, not in the order of the line string. The point behind a point at index i is at index i-1.

Parameters

[in]	line_string	line string
[in]	point	other point
[in]	idx_closest	index of closest point in line string
[in]	behind	whether to consider points behind (or ahead of) the given point

Returns

Definition at line 144 of file utils.cpp.

                                                                   {
  if (line_string.empty()) {
    return 0;
  }
 
  size_t new_idx_closest = idx_closest;
  Eigen::Vector2d closest_point_to_next;
  const Eigen::Vector2d closest_point_to_point = point - line_string[idx_closest];
  if (idx_closest + 1 < line_string.size()) {
    closest_point_to_next = line_string[idx_closest + 1] - line_string[idx_closest];
  } else if (idx_closest > 0) {
    closest_point_to_next = line_string[idx_closest] - line_string[idx_closest - 1];
  } else {
    return idx_closest;
  }
 
  // use angle to check if closest point is behind or ahead of the given point
  const double angle = angleBetweenVectors(closest_point_to_point, closest_point_to_next);
  if (behind && std::abs(angle) > M_PI_2) {
    new_idx_closest = idx_closest > 0 ? idx_closest - 1 : 0;
  } else if (!behind && std::abs(angle) < M_PI_2) {
    new_idx_closest = idx_closest + 1;
  } else {
    new_idx_closest = idx_closest;
  }
  new_idx_closest = std::clamp(new_idx_closest, size_t{0}, line_string.size() - 1);
 
  return new_idx_closest;
}

createMinimalRouteElement()

route_planning_msgs::msg::RouteElement lanelet2_route_planning::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 )

Create a minimal route element message.

A minimal valid route element only has a single lane element with a reference pose.

Parameters

[in]	position	position
[in]	orientation	orientation
[in]	s	accumulated distance along route
[in]	will_change_suggested_lane	whether the lane will change to the next route element
[in]	speed_limit	speed limit

Returns

minimal route element

Definition at line 345 of file utils.cpp.

                                                                                      {
  // create RouteElement
  route_planning_msgs::msg::RouteElement route_element_msg;
  route_element_msg.suggested_lane_idx = 0;
  route_element_msg.will_change_suggested_lane = will_change_suggested_lane;
  route_element_msg.s = s;
  route_element_msg.is_enriched = false;
  // route_element_msg.left_boundary not set in global route
  // route_element_msg.right_boundary not set in global route
  // route_element_msg.regulatory_elements not set in global route
 
  // create LaneElement
  route_planning_msgs::msg::LaneElement lane_element_msg;
  lane_element_msg.reference_pose.position = position;
  lane_element_msg.reference_pose.orientation = orientation;
  // lane_element_msg.left_boundary not set in global route
  // lane_element_msg.right_boundary not set in global route
  lane_element_msg.speed_limit = speed_limit;
  // lane_element_msg.regulatory_element_idcs not set in global route
  lane_element_msg.following_lane_idx = 0;
  lane_element_msg.has_following_lane_idx = !will_change_suggested_lane;
  route_element_msg.lane_elements.push_back(lane_element_msg);
 
  return route_element_msg;
}

distanceRemaining()

double lanelet2_route_planning::distanceRemaining

(

const route_planning_msgs::msg::Route &

route

)

Computes the remaining distance along the route.

Accurate to the level of route elements. Ignores route overshoot.

Parameters

[in]

route

route

Returns

remaining distance [m]

Definition at line 1004 of file utils.cpp.

                                                                   {
  if (route.current_route_element_idx >= route.route_elements.size() ||
      route.destination_route_element_idx >= route.route_elements.size()) {
    return 0.0;
  }
  double to_current = route.route_elements[route.current_route_element_idx].s;
  double to_destination = route.route_elements[route.destination_route_element_idx].s;
  double remaining = to_destination - to_current;
  return remaining;
}

distanceTraveled()

double lanelet2_route_planning::distanceTraveled

(

const route_planning_msgs::msg::Route &

route

)

Computes the traveled distance along the route.

Accurate to the level of route elements. Ignores route undershoot.

Parameters

[in]

route

route

Returns

traveled distance [m]

Definition at line 993 of file utils.cpp.

                                                                  {
  if (route.current_route_element_idx >= route.route_elements.size() ||
      route.starting_route_element_idx >= route.route_elements.size()) {
    return 0.0;
  }
  double to_current = route.route_elements[route.current_route_element_idx].s;
  double ahead_of_starting_point = route.route_elements[route.starting_route_element_idx].s;
  double traveled = to_current - ahead_of_starting_point;
  return traveled;
}

egoPosition()

geometry_msgs::msg::Point lanelet2_route_planning::egoPosition

(

const perception_msgs::msg::EgoData &

ego_data

)

Extracts an EgoData position as a ROS point.

Parameters

[in]

ego_data

ego data

Returns

ego data position

Definition at line 53 of file conversions.cpp.

                                                                             {
  geometry_msgs::msg::Point position;
  position.x = perception_msgs::object_access::getX(ego_data);
  position.y = perception_msgs::object_access::getY(ego_data);
  position.z = perception_msgs::object_access::getZ(ego_data);
  return position;
}

estimateRemainingTime()

double lanelet2_route_planning::estimateRemainingTime	(	const route_planning_msgs::msg::Route &	route,
		const double	reference_speed = 50.0 / 3.6 )

Estimate remaining time for a route based on speed limits.

Parameters

[in]	route	route
[in]	reference_speed	reference speed if speed limit is not set [m/s]

Returns

estimated remaining time [s]

Definition at line 1015 of file utils.cpp.

                                                                                                     {
  double remaining_time = 0.0;
  for (size_t r = route.current_route_element_idx; r < route.route_elements.size() - 1; ++r) {
    const auto& route_element = route.route_elements[r];
    const auto& next_route_element = route.route_elements[r + 1];
    double speed_limit = route_planning_msgs::route_access::getSuggestedLaneElement(route_element).speed_limit;
    if (speed_limit == 0) {
      speed_limit = reference_speed;
    }
    if (speed_limit > 0) {
      remaining_time += (next_route_element.s - route_element.s) / speed_limit;
    }
  }
  return remaining_time;
}

extractDrivableSpace()

std::pair< Eigen::Vector2d, Eigen::Vector2d > lanelet2_route_planning::extractDrivableSpace	(	const lanelet::LineStringLayer &	line_string_layer,
		const PointSequence &	point_sequence,
		const double	max_distance )

Extracts drivable space boundaries for a route element.

Drivable space is defined as the area around the current point that is physically drivable. To extract it, the intersection of the normal to the current point with all existing map line strings is computed. The outermost intersection point that is not drivable anymore is considered the boundary. If no such point is found, the maximum distance is used.

Parameters

[in]	line_string_layer	map line string layer
[in]	point_sequence	point sequence (should be centerline of main lanelet)
[in]	max_distance	maximum distance to left/right drivable space bounds, if not otherwise restricted

Returns

left and right drivable space bounds

Definition at line 375 of file utils.cpp.

                                                                                            {
  // find all line strings within max_distance
  const lanelet::BasicLineString2d line_to_search_around = {point_sequence.current, point_sequence.next};
  std::vector<std::pair<double, lanelet::ConstLineString3d>> line_strings_and_distances =
      lanelet::geometry::findWithin2d(line_string_layer, line_to_search_around, max_distance);
 
  // collect all intersections of the normal through the current point with nearby line strings
  const std::vector<Eigen::Vector2d> normal_line = {
      point_sequence.current,
      point_sequence.current + normalOfPointAlongLineString(point_sequence.current, point_sequence.prev, point_sequence.next)};
  std::vector<std::pair<Eigen::Vector2d, size_t>> projected_points_and_line_string_idcs;
  for (size_t l = 0; l < line_strings_and_distances.size(); ++l) {
    const auto& line_string = line_strings_and_distances[l].second;
    const std::vector<Eigen::Vector2d> line_string_2d = to2d(toEigen(line_string.basicLineString()));
    for (size_t i = 1; i < line_string_2d.size(); ++i) {
      const std::vector<Eigen::Vector2d> line_segment = {line_string_2d[i - 1], line_string_2d[i]};
      auto intersection = intersectionOfLines(normal_line, line_segment);
      if (!intersection || !intersection->intersects_line2) {
        continue;
      }
 
      const Eigen::Vector2d& projected_point = intersection->intersection;
      if ((projected_point - point_sequence.current).norm() > max_distance) {
        continue;
      }
 
      projected_points_and_line_string_idcs.emplace_back(projected_point, l);
    }
  }
 
  // sort projected points by distance to current point
  std::sort(projected_points_and_line_string_idcs.begin(), projected_points_and_line_string_idcs.end(),
            [&point_sequence](const auto& a, const auto& b) {
              return (a.first - point_sequence.current).norm() < (b.first - point_sequence.current).norm();
            });
 
  // split projected points into those left and those right of current point
  const Eigen::Vector2d tangent = tangentOfPointAlongLineString(point_sequence.current, point_sequence.prev, point_sequence.next);
  std::vector<std::pair<Eigen::Vector2d, size_t>> left_projected_points_and_line_string_idcs,
      right_projected_points_and_line_string_idcs;
  for (const auto& projected_point_and_line_string_idx : projected_points_and_line_string_idcs) {
    // determine left/right based on angle between tangent and vector to projected point
    const Eigen::Vector2d point_to_projected_point = projected_point_and_line_string_idx.first - point_sequence.current;
    const double angle = angleBetweenVectors(tangent, point_to_projected_point);
    if (angle > 0) {
      left_projected_points_and_line_string_idcs.emplace_back(projected_point_and_line_string_idx);
    } else {
      right_projected_points_and_line_string_idcs.emplace_back(projected_point_and_line_string_idx);
    }
  }
 
  // find drivable space bounds by following projected points until corresponding line string is not passable anymore
  Eigen::Vector2d drivable_space_left, drivable_space_right;
  bool is_drivable_space_left_limited_by_line_strings = false;
  bool is_drivable_space_right_limited_by_line_strings = false;
  for (const auto& projected_point_and_line_string_idx : left_projected_points_and_line_string_idcs) {
    const auto& line_string = line_strings_and_distances[projected_point_and_line_string_idx.second].second;
    if (!isLineStringDrivable(line_string)) {
      drivable_space_left = projected_point_and_line_string_idx.first;
      is_drivable_space_left_limited_by_line_strings = true;
      break;
    }
  }
  for (const auto& projected_point_and_line_string_idx : right_projected_points_and_line_string_idcs) {
    const auto& line_string = line_strings_and_distances[projected_point_and_line_string_idx.second].second;
    if (!isLineStringDrivable(line_string)) {
      drivable_space_right = projected_point_and_line_string_idx.first;
      is_drivable_space_right_limited_by_line_strings = true;
      break;
    }
  }
 
  // if drivable space is not limited by line strings, use maximum distance
  const Eigen::Vector2d normal = normalOfPointAlongLineString(point_sequence.current, point_sequence.prev, point_sequence.next);
  if (!is_drivable_space_left_limited_by_line_strings) {
    drivable_space_left = point_sequence.current - normal * max_distance;
  }
  if (!is_drivable_space_right_limited_by_line_strings) {
    drivable_space_right = point_sequence.current + normal * max_distance;
  }
 
  return {drivable_space_left, drivable_space_right};
}

extractLocalRouteWindow()

LocalRouteWindow lanelet2_route_planning::extractLocalRouteWindow	(	const route_planning_msgs::msg::Route &	full_route,
		size_t	current_global_idx,
		double	distance_behind,
		double	distance_ahead )

Extracts a local route window while preserving absolute route data.

Parameters

[in]	full_route	complete route
[in]	current_global_idx	current element index in full_route
[in]	distance_behind	distance to include behind the current element [m]
[in]	distance_ahead	distance to include ahead of the current element [m]

Returns

extracted local route and its global offset

Definition at line 1169 of file utils.cpp.

                                                                      {
  LocalRouteWindow result;
  result.route.header = full_route.header;
  result.route.destination = full_route.destination;
  result.route.intermediate_destinations = full_route.intermediate_destinations;
  const auto& full_route_elements = full_route.route_elements;
  if (current_global_idx >= full_route_elements.size()) {
    result.route.route_elements.clear();
    return result;
  }
 
  // find the route-element bounds by their absolute distances
  const double current_s = full_route_elements[current_global_idx].s;
  const auto first_it = std::lower_bound(
      full_route_elements.begin(), full_route_elements.end(), current_s - distance_behind,
      [](const route_planning_msgs::msg::RouteElement& route_element, const double s) { return route_element.s < s; });
  const auto last_it = std::upper_bound(
      full_route_elements.begin(), full_route_elements.end(), current_s + distance_ahead,
      [](const double s, const route_planning_msgs::msg::RouteElement& route_element) { return s < route_element.s; });
  result.first_global_idx = std::distance(full_route_elements.begin(), first_it);
  const size_t last_global_idx = std::distance(full_route_elements.begin(), last_it);
  result.route.route_elements.assign(first_it, last_it);
 
  // remap route indices from the complete route to the local window
  const auto remap_global_idx = [first_global_idx = result.first_global_idx, last_global_idx](const uint64_t global_idx) {
    return (global_idx >= first_global_idx && global_idx < last_global_idx)
               ? global_idx - first_global_idx
               : route_planning_msgs::msg::Route::INVALID_ROUTE_ELEMENT_IDX;
  };
  result.route.starting_route_element_idx = remap_global_idx(full_route.starting_route_element_idx);
  result.route.current_route_element_idx = current_global_idx - result.first_global_idx;
  result.route.destination_route_element_idx = remap_global_idx(full_route.destination_route_element_idx);
  return result;
}

extractRegulatoryElements()

ExtractRegulatoryElementsResult lanelet2_route_planning::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 )

Extracts regulatory element information for a route element.

Regulatory elements are queried from a lanelet and its adjacent lanelets. They are only considered if their reference line intersects with the given point sequence, which should be the centerline of the main lanelet. This way, regulatory elements are assignable to the closest route element. Note that the assignment to adjacent lanes is also based on the intersection with the single given point sequence.

Parameters

[in]	lanelet	lanelet
[in]	adjacent_left_lanelets	left adjacent lanelets
[in]	adjacent_right_lanelets	right adjacent lanelets
[in]	point_sequence	point sequence (should be centerline of main lanelet)

Returns

regulatory element information

Definition at line 481 of file utils.cpp.

                                                                                               {
  // init result
  ExtractRegulatoryElementsResult result;
  result.adjacent_left_regulatory_element_idcs.resize(adjacent_left_lanelets.size());
  result.adjacent_right_regulatory_element_idcs.resize(adjacent_right_lanelets.size());
 
  // gather lanelets in single vector (left adjacent, current, right adjacent)
  std::vector<lanelet::ConstLanelet> lanelets = adjacent_left_lanelets;
  lanelets.push_back(lanelet);
  lanelets.insert(lanelets.end(), adjacent_right_lanelets.begin(), adjacent_right_lanelets.end());
 
  // loop over lanelets
  std::unordered_map<size_t, size_t> regulatory_element_msg_idx_by_id;
  for (size_t l = 0; l < lanelets.size(); ++l) {
    const auto& current_lanelet = lanelets[l];
 
    // loop over regulatory elements of lanelet
    const auto regulatory_elements = current_lanelet.regulatoryElements();
    for (const auto& regulatory_element : regulatory_elements) {
      // create RegulatoryElement
      route_planning_msgs::msg::RegulatoryElement regulatory_element_msg;
      regulatory_element_msg.has_validity_stamp = false;
      regulatory_element_msg.validity_stamp = builtin_interfaces::msg::Time();
 
      // extract reference line
      if (auto reference_line = regulatoryElementReferenceLine(regulatory_element)) {
        regulatory_element_msg.reference_line = *reference_line;
 
        // only consider regulatory element if reference line intersects with point sequence
        std::vector<Eigen::Vector2d> reference_line_2d = {toEigen2d(reference_line->at(0)), toEigen2d(reference_line->at(1))};
        std::vector<Eigen::Vector2d> line_to_next_point = {point_sequence.current, point_sequence.next};
        std::vector<Eigen::Vector2d> line_to_prev_point = {point_sequence.current, point_sequence.prev};
        if (auto result = intersectionOfLines(reference_line_2d, line_to_next_point)) {
          if (!result->intersects_line2) {
            if (auto inner_result = intersectionOfLines(reference_line_2d, line_to_prev_point)) {
              if (!inner_result->intersects_line2) {
                continue;
              }
            }
          }
        }
      } else {
        continue;
      }
 
      // extract sign positions and type
      regulatory_element_msg.positions = regulatoryElementPositions(regulatory_element);
      std::tie(regulatory_element_msg.type, regulatory_element_msg.meta_value) = regulatoryElementType(regulatory_element);
 
      // check if regulatory element has already been extracted (by another lanelet)
      size_t regulatory_element_msg_idx = 0;
      if (regulatory_element_msg_idx_by_id.count(regulatory_element->id()) > 0) {
        regulatory_element_msg_idx = regulatory_element_msg_idx_by_id[regulatory_element->id()];
      } else {
        // add regulatory element to result
        regulatory_element_msg_idx = result.regulatory_element_msgs.size();
        regulatory_element_msg_idx_by_id[regulatory_element->id()] = regulatory_element_msg_idx;
        result.regulatory_element_msgs.push_back(regulatory_element_msg);
      }
 
      // assign regulatory element to respective lanelet in result
      if (l < adjacent_left_lanelets.size()) {
        result.adjacent_left_regulatory_element_idcs[l].push_back(regulatory_element_msg_idx);
      } else if (l < adjacent_left_lanelets.size() + 1) {
        result.regulatory_element_idcs.push_back(regulatory_element_msg_idx);
      } else {
        size_t l_right = l - adjacent_left_lanelets.size() - 1;
        result.adjacent_right_regulatory_element_idcs[l_right].push_back(regulatory_element_msg_idx);
      }
    }
  }
 
  return result;
}

followLaneletsAlongRoutingGraph()

lanelet::ConstLanelet lanelet2_route_planning::followLaneletsAlongRoutingGraph	(	const lanelet::routing::RoutingGraphUPtr &	routing_graph,
		const lanelet::ConstLanelet &	lanelet,
		const Eigen::Vector2d &	position,
		const double	distance )

Follows a lanelet's and following lanelets' centerline for a given distance.

If lanelets cannot be followed anymore, returns the last lanelet.

Parameters

[in]	routing_graph	routing graph
[in]	lanelet	current lanelet
[in]	position	position on current lanelet
[in]	distance	distance to follow lanelets (may be negative to follow in opposite direction)

Returns

followed lanelet

Definition at line 914 of file utils.cpp.

                                                                             {
  lanelet::ConstLanelet followed_lanelet = lanelet;
  double remaining_length = 0.0;
  if (distance > 0) {
    remaining_length = static_cast<double>(lanelet::geometry::length(lanelet.centerline2d())) -
                       lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), position).length;
  } else {
    remaining_length = lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), position).length;
  }
  double remaining_distance = std::abs(distance);
 
  while (remaining_distance > remaining_length) {
    lanelet::ConstLanelets next_lanelets;
    if (distance > 0) {
      next_lanelets = routing_graph->following(followed_lanelet, false);
    } else {
      next_lanelets = routing_graph->previous(followed_lanelet);
    }
    if (next_lanelets.empty()) {
      break;
    }
    followed_lanelet = next_lanelets.front();
    remaining_distance -= remaining_length;
    remaining_length = static_cast<double>(lanelet::geometry::length(followed_lanelet.centerline2d()));
  }
 
  return followed_lanelet;
}

getRoute()

std::optional< lanelet::routing::Route > lanelet2_route_planning::getRoute	(	const lanelet::routing::RoutingGraphUPtr &	routing_graph,
		const std::vector< lanelet::ConstLanelet > &	route_lanelets )

Computes a route from start to destination along intermediate destinations.

Parameters

[in]	routing_graph	routing graph
[in]	route_lanelets	lanelets to plan along, starting with start lanelet, ending with destination lanelet

Returns

route if found, otherwise std::nullopt

Definition at line 26 of file utils.cpp.

                                                                                                      {
  if (route_lanelets.empty()) {
    return std::nullopt;
  }
 
  const lanelet::ConstLanelet& start_lanelet = route_lanelets.front();
  std::vector<lanelet::ConstLanelet> intermediate_lanelets(route_lanelets.begin() + 1, route_lanelets.end() - 1);
  const lanelet::ConstLanelet& destination_lanelet = route_lanelets.back();
 
  // compute default route
  const int routing_cost_id = 0;  // RoutingCostDistance
  const bool with_lane_changes = true;
  auto route =
      routing_graph->getRouteVia(start_lanelet, intermediate_lanelets, destination_lanelet, routing_cost_id, with_lane_changes);
 
  // compute route alternatives with inverted start/destination lanelets (useful, if they are bidirectional)
  auto route_alternative1 =
      routing_graph->getRouteVia(start_lanelet.invert(), intermediate_lanelets, destination_lanelet, routing_cost_id);
  auto route_alternative2 =
      routing_graph->getRouteVia(start_lanelet, intermediate_lanelets, destination_lanelet.invert(), routing_cost_id);
  auto route_alternative3 =
      routing_graph->getRouteVia(start_lanelet.invert(), intermediate_lanelets, destination_lanelet.invert(), routing_cost_id);
  std::vector<lanelet::routing::Route> route_alternatives;
  if (route) route_alternatives.push_back(std::move(*route));
  if (route_alternative1) route_alternatives.push_back(std::move(*route_alternative1));
  if (route_alternative2) route_alternatives.push_back(std::move(*route_alternative2));
  if (route_alternative3) route_alternatives.push_back(std::move(*route_alternative3));
 
  // select shortest route
  auto shortest_route_ptr = std::min_element(
      route_alternatives.begin(), route_alternatives.end(),
      [](const lanelet::routing::Route& a, const lanelet::routing::Route& b) { return a.length2d() < b.length2d(); });
 
  if (shortest_route_ptr == route_alternatives.end()) {
    return std::nullopt;
  }
  auto shortest_route = std::optional<lanelet::routing::Route>(std::move(*shortest_route_ptr));
  return shortest_route;
}

getTrafficRules()

lanelet::traffic_rules::TrafficRulesPtr lanelet2_route_planning::getTrafficRules

(

)

Get traffic rules.

Returns

traffic rules

Definition at line 879 of file utils.cpp.

                                                      {
  auto location = lanelet::Locations::Germany;                      // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay)
  auto vehicle_type = std::string(lanelet::Participants::Vehicle);  // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay)
  return lanelet::traffic_rules::TrafficRulesFactory::create(location, vehicle_type);
}

indexOfLineStringPointClosestToPoint()

size_t lanelet2_route_planning::indexOfLineStringPointClosestToPoint	(	const std::vector< Eigen::Vector2d > &	line_string,
		const Eigen::Vector2d &	point,
		const bool	consider_order = false,
		const bool	behind = true )

Finds the index of a point in a line string that is closest to another point.

If consider_order, then only the points behind or ahead of the projection of the given point are considered.

Parameters

[in]	line_string	line string
[in]	point	other point
[in]	consider_order	whether to consider the order of points in the line string
[in]	behind	whether to consider points behind (or ahead of) the given point

Returns

index of closest point

Definition at line 67 of file utils.cpp.

                                                               {
  if (line_string.empty()) {
    return 0;
  }
 
  // loop over all points in line string to find closest one to given point
  size_t idx_closest = 0;
  double min_distance = std::numeric_limits<double>::infinity();
  for (size_t i = 0; i < line_string.size(); ++i) {
    double distance = (line_string[i] - point).norm();
    if (distance < min_distance) {
      min_distance = distance;
      idx_closest = i;
    }
  }
 
  // if considering order, make sure to return the point behind or ahead of the given point
  if (consider_order) {
    idx_closest = considerOrderForPointMatchedToLineString(line_string, point, idx_closest, behind);
  }
 
  return idx_closest;
}

intersectionOfLines()

std::optional< IntersectionOfLinesResult > lanelet2_route_planning::intersectionOfLines	(	const std::vector< Eigen::Vector2d > &	line1,
		const std::vector< Eigen::Vector2d > &	line2 )

Computes the intersection of two 2D lines.

Parameters

[in]	line1	first line defined by two points
[in]	line2	second line defined by two points

Returns

intersection point and whether intersection is on line segments

Definition at line 87 of file geometry.cpp.

                                                                                                    {
  IntersectionOfLinesResult result;
 
  // check if lines are valid
  if (line1.size() < 2 || line2.size() < 2) {
    return std::nullopt;
  }
 
  // define straight lines (a1 x + b1 y = c1) and (a2 x + b2 y = c2) through line points
  const double a1 = line1[1].y() - line1[0].y();
  const double b1 = line1[0].x() - line1[1].x();
  const double c1 = line1[0].x() * line1[1].y() - line1[1].x() * line1[0].y();
  const double a2 = line2[1].y() - line2[0].y();
  const double b2 = line2[0].x() - line2[1].x();
  const double c2 = line2[0].x() * line2[1].y() - line2[1].x() * line2[0].y();
 
  // find intersection of both lines by solving for x and y
  const double det = a1 * b2 - a2 * b1;
  if (det != 0) {
    const double x = (b2 * c1 - b1 * c2) / det;
    const double y = (a1 * c2 - a2 * c1) / det;
    result.intersection = Eigen::Vector2d(x, y);
 
    // check if intersection point is within line segments
    result.intersects_line1 = (x >= std::min(line1[0].x(), line1[1].x()) && x <= std::max(line1[0].x(), line1[1].x()) &&
                               y >= std::min(line1[0].y(), line1[1].y()) && y <= std::max(line1[0].y(), line1[1].y()));
    result.intersects_line2 = (x >= std::min(line2[0].x(), line2[1].x()) && x <= std::max(line2[0].x(), line2[1].x()) &&
                               y >= std::min(line2[0].y(), line2[1].y()) && y <= std::max(line2[0].y(), line2[1].y()));
  } else {
    return std::nullopt;
  }
 
  return result;
}

isLineStringDrivable()

bool lanelet2_route_planning::isLineStringDrivable

(

const lanelet::ConstLineString3d &

line_string

)

Checks if lanelet line string has a type that is considered drivable.

Drivable in the sense of being able to physically cross it, not whether it is allowed to cross it.

The following types are considered drivable:

• arrow
• bike_marking
• centerline
• curbstone (only if subtype is 'low')
• lane_center
• line_thick
• line_thin
• pedestrian_marking
• roadpainting
• stop_line
• traffic_light
• virtual
• zebra_marking

Additionally, the extra attribute 'HoldingLine' is considered drivable.

Parameters

[in]

line_string

line string

Returns

whether line string is drivable

Definition at line 461 of file utils.cpp.

                                                                       {
  const std::unordered_set<std::string> drivable_types = {"arrow",        "bike_marking", "centerline",    "curbstone",
                                                          "lane_center",  "line_thick",   "line_thin",     "pedestrian_marking",
                                                          "roadpainting", "stop_line",    "traffic_light", "virtual",
                                                          "zebra_marking"};
  if (line_string.hasAttribute("type")) {
    std::string type = line_string.attribute("type").value();
    if (drivable_types.count(type) > 0) {
      if (type == "curbstone") {
        if (!line_string.hasAttribute("subtype") || line_string.attribute("subtype").value() != "low") {
          return false;
        }
      }
      return true;
    }
    return line_string.hasAttribute("HoldingLine");
  }
  return false;
}

laneBoundaryType()

uint8_t lanelet2_route_planning::laneBoundaryType

(

const lanelet::ConstLineString2d &

line

)

Extracts the lane boundary type of a lanelet line.

Lane boundary type as in route_planning_msgs::msg::LaneBoundary.

Parameters

[in]

line

lanelet line

Returns

lane boundary type

Definition at line 666 of file utils.cpp.

                                                               {
  uint8_t lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_UNKNOWN;
 
  // get type attribute
  lanelet::Attribute type;
  if (line.hasAttribute("type")) {
    type = line.attribute("type");
  } else {
    lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_UNKNOWN;
    return lane_boundary_type;
  }
 
  // map lanelet type to lane boundary type
  if (type == "road_boarder" || type == "barrier") {
    lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_CROSSING_RESTRICTED;
  } else if (type == "line_thin" || type == "line_thick") {
    lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_UNKNOWN;
    if (line.hasAttribute("subtype")) {
      lanelet::Attribute subtype = line.attribute("subtype");
      if (subtype == "solid" || subtype == "solid_solid") {
        lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_CROSSING_RESTRICTED;
      } else if (subtype == "dashed") {
        lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_CROSSING_ALLOWED;
      } else if (subtype == "dashed_solid") {
        lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_CROSSING_ALLOWED_FROM_LEFT;
      } else if (subtype == "solid_dashed") {
        lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_CROSSING_ALLOWED_FROM_RIGHT;
      }
    }
  } else if (type == "virtual") {
    lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_UNKNOWN;
  } else {
    lane_boundary_type = route_planning_msgs::msg::LaneBoundary::TYPE_UNKNOWN;
  }
 
  return lane_boundary_type;
}

laneletAtPoint()

std::optional< lanelet::ConstLanelet > lanelet2_route_planning::laneletAtPoint	(	const Eigen::Vector2d &	point,
		const lanelet::LaneletMapConstPtr &	map,
		const std::optional< lanelet::traffic_rules::TrafficRulesPtr >	traffic_rules = std::nullopt )

Find lanelet at arbitrary point.

Finds closest passable lanelet within 10m of the given point. Only considers the 5 closest lanelets.

Parameters

[in]	point	point
[in]	map	map
[in]	traffic_rules	traffic rules

Returns

matching lanelet

Definition at line 885 of file utils.cpp.

                                                                                                                          {
  // parameters for lanelet matching
  const unsigned int number_of_nearest_lanelets = 5;
  const double max_lanelet_matching_distance = 5.0;
 
  // find nearest lanelets
  std::vector<std::pair<double, lanelet::ConstLanelet>> nearest_lanelets =
      lanelet::geometry::findNearest(map->laneletLayer, point, number_of_nearest_lanelets);
 
  // find best matching lanelet
  if (traffic_rules) {
    sortLaneletsByMatchingCost(point, nearest_lanelets, traffic_rules);
    for (const auto& nearest_lanelet : nearest_lanelets) {
      if (nearest_lanelet.first <= max_lanelet_matching_distance && traffic_rules.value()->canPass(nearest_lanelet.second)) {
        return nearest_lanelet.second;
      }
    }
  } else if (!nearest_lanelets.empty()) {
    sortLaneletsByMatchingCost(point, nearest_lanelets);
    if (nearest_lanelets[0].first <= max_lanelet_matching_distance) {
      return nearest_lanelets[0].second;
    }
  }
 
  return std::nullopt;
}

matchPointToLineString()

size_t lanelet2_route_planning::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 )

Finds the index of a point in a line string that is locally closest to another point.

The purpose of this function is not to find the globally closest point, but the locally closest one. By locally, what is meant is that points closer in order to idx_indication are preferred over points that may actually be closer in distance.

The range of points 10m before and after idx_indication is considered. A maximum distance of 10m is used to match to this range of points, otherwise globally closer ones are preferred.

If consider_order, then only the points behind or ahead of the projection of the given point are considered.

Parameters

[in]	line_string	line string
[in]	point	other point
[in]	idx_indication	index of point in line string to indicate the range of points to be considered
[in]	consider_order	whether to consider the order of points in the line string
[in]	behind	whether to consider points behind (or ahead of) the given point

Returns

index of locally closest point

Definition at line 94 of file utils.cpp.

                                                 {
  if (line_string.empty()) {
    return 0;
  }
 
  // constants
  const double max_delta_s = 10.0;
  const double max_local_distance = 10.0;
 
  size_t idx_closest = 0;
  double min_distance = std::numeric_limits<double>::infinity();
  const size_t start_idx = std::min(idx_indication, line_string.size() - 1);
 
  // loop over points in front of and behind the given index
  for (int direction : {1, -1}) {
    double delta_s = 0.0;
    size_t idx = start_idx;
    // only check points within the local range of max_delta_s
    while (delta_s <= max_delta_s && idx < line_string.size()) {
      double distance = (line_string[idx] - point).norm();
      if (distance < min_distance) {
        min_distance = distance;
        idx_closest = idx;
      }
      if (direction == 1) {
        if (idx + 1 >= line_string.size()) break;
        delta_s += (line_string[idx + 1] - line_string[idx]).norm();
      } else {
        if (idx == 0) break;
        delta_s += (line_string[idx] - line_string[idx - 1]).norm();
      }
      idx += direction;
    }
  }
 
  // check if closest local point is within max distance, else find globally closest point
  if (min_distance > max_local_distance) {
    idx_closest = indexOfLineStringPointClosestToPoint(line_string, point, consider_order, behind);
  } else if (consider_order) {
    // if considering order, make sure to return the point behind or ahead of the given point
    idx_closest = considerOrderForPointMatchedToLineString(line_string, point, idx_closest, behind);
  }
 
  return idx_closest;
}

normalOfPointAlongLineString()

Eigen::Vector2d lanelet2_route_planning::normalOfPointAlongLineString	(	const Eigen::Vector2d &	point,
		const Eigen::Vector2d &	prev_point,
		const Eigen::Vector2d &	next_point )

Computes a unit vector normal to a point along a line string.

The normal is normal to the tangent, see tangentOfPointAlongLineString.

Parameters

[in]	point	point along line string
[in]	prev_point	previous point along line string
[in]	next_point	next point along line string

Returns

unit normal vector

Definition at line 143 of file geometry.cpp.

                                                                              {
  Eigen::Vector2d tangent = tangentOfPointAlongLineString(point, prev_point, next_point);
  Eigen::Vector2d normal = Eigen::Vector2d(tangent.y(), -tangent.x());
 
  return normal;
}

postprocessRouteMessage()

void lanelet2_route_planning::postprocessRouteMessage	(	route_planning_msgs::msg::Route &	route_msg,
		std::vector< std::vector< int > > &	suggested_turn_signal_distance_ahead_by_route_element_by_lane_element )

Postprocesses a route message, filling missing information that can be inferred from other message contents.

This includes:

• orientation of each lane element's reference pose based on preceding and following points
• propagation of suggested turn signals backwards along the route based on distance ahead information

Parameters

[in,out]	route_msg	route message to postprocess
[in]	suggested_turn_signal_distance_ahead_by_route_element_by_lane_element	suggested turn signal distances ahead by route element and lane element indices

Definition at line 1031 of file utils.cpp.

                                                                                                      {
  // loop over route elements to set orientations and following lane indices
  std::vector<route_planning_msgs::msg::RouteElement>& route_elements = route_msg.route_elements;
  for (size_t r = 0; r < route_elements.size(); ++r) {
    // get current, previous and next route element
    auto& route_element = route_elements[r];
    auto& prev_route_element = (r > 0) ? route_elements[r - 1] : route_element;
    const auto& next_route_element = (r < route_elements.size() - 1) ? route_elements[r + 1] : route_element;
 
    // fix following lane index at the last non-enriched route element before route elements are enriched
    if (r > 0) {
      if (!prev_route_element.is_enriched) {
        prev_route_element.lane_elements[0].has_following_lane_idx = true;
        prev_route_element.lane_elements[0].following_lane_idx = route_element.suggested_lane_idx;
      }
    }
 
    // loop over lane elements of current route element
    for (size_t l = 0; l < route_element.lane_elements.size(); ++l) {
      // get current, previous and next lane element
      auto& lane_element = route_element.lane_elements[l];
      const auto prev_lane_element_opt = route_planning_msgs::route_access::getPrecedingLaneElement(l, prev_route_element);
      const auto next_lane_element_opt =
          route_planning_msgs::route_access::getFollowingLaneElement(lane_element, next_route_element);
 
      // find current, previous and next points to compute orientation
      const auto point = toEigen2d(lane_element.reference_pose.position);
      const bool changes_lane_from_prev_point = prev_route_element.will_change_suggested_lane;
      const bool changes_lane_to_next_point = next_route_element.will_change_suggested_lane;
      const auto prev_point_for_orientation = (changes_lane_from_prev_point || !prev_lane_element_opt)
                                                  ? point
                                                  : toEigen2d(prev_lane_element_opt->reference_pose.position);
      const auto next_point_for_orientation = (changes_lane_to_next_point || !next_lane_element_opt)
                                                  ? point
                                                  : toEigen2d(next_lane_element_opt->reference_pose.position);
 
      // compute orientation of current point
      const auto orientation = tangentOfPointAlongLineString(point, prev_point_for_orientation, next_point_for_orientation);
      lane_element.reference_pose.orientation = toRosQuaternion(orientation);
    }
  }
 
  // loop over route elements in reverse to propagate suggested turn signals backwards
  for (int r = static_cast<int>(route_elements.size()) - 1; r >= 0; --r) {
    auto& route_element = route_elements[r];
    if (!route_element.is_enriched) continue;  // only handle turn signal information for enriched route elements
 
    // loop over lane elements of current route element
    for (size_t l = 0; l < route_element.lane_elements.size(); ++l) {
      auto& lane_element = route_element.lane_elements[l];
 
      if (suggested_turn_signal_distance_ahead_by_route_element_by_lane_element[r].empty()) continue;
      int suggested_turn_signal_distance_ahead = suggested_turn_signal_distance_ahead_by_route_element_by_lane_element[r][l];
 
      // get preceding lane element
      if (r <= 0) break;
      int curr_r = r;
      auto* curr_lane_element = &lane_element;
      auto* prev_route_element = &route_elements[r - 1];
      auto prev_lane_element_idx_opt = route_planning_msgs::route_access::getPrecedingLaneElementIdx(l, *prev_route_element);
 
      // iterate over preceding lane elements within suggested distance to set suggested turn signal
      while (suggested_turn_signal_distance_ahead > 0 && prev_lane_element_idx_opt) {
        auto& prev_lane_element = prev_route_element->lane_elements[*prev_lane_element_idx_opt];
        if (!prev_route_element->is_enriched) break;  // only propagate through enriched route elements
 
        // stop if preceding lane element has its own suggested turn signal information
        if (!suggested_turn_signal_distance_ahead_by_route_element_by_lane_element[curr_r - 1].empty()) {
          int prev_suggested_turn_signal_distance_ahead =
              suggested_turn_signal_distance_ahead_by_route_element_by_lane_element[curr_r - 1][*prev_lane_element_idx_opt];
          if (prev_suggested_turn_signal_distance_ahead >= 0) {
            break;
          }
        }
 
        // check distance to preceding lane element
        const auto point = toEigen2d(curr_lane_element->reference_pose.position);
        const auto prev_point = toEigen2d(prev_lane_element.reference_pose.position);
        const double distance_to_prev_point = (point - prev_point).norm();
        if (distance_to_prev_point > suggested_turn_signal_distance_ahead) {
          break;  // stop if distance to preceding point exceeds remaining distance ahead
        }
 
        // set suggested turn signal of preceding lane element
        prev_lane_element.suggested_turn_signal = lane_element.suggested_turn_signal;
 
        // update remaining distance and move to next preceding lane element
        suggested_turn_signal_distance_ahead -= static_cast<uint8_t>(std::round(distance_to_prev_point));
        curr_r -= 1;
        if (curr_r <= 0) break;
        curr_lane_element = &prev_lane_element;
        prev_route_element = &route_elements[curr_r - 1];
        prev_lane_element_idx_opt =
            route_planning_msgs::route_access::getPrecedingLaneElementIdx(*prev_lane_element_idx_opt, *prev_route_element);
      }
    }
  }
}

projectPointToLaneletLines()

std::vector< ProjectedLaneletPoints > lanelet2_route_planning::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") )

Projects a point to the centerline and bounds of a set of lanelets.

For projection, projectPointToLineStringAlongNormal is used.

Parameters

[in]	point	point
[in]	prev_point	previous point
[in]	next_point	next point
[in]	lanelets	lanelets
[in]	logger	logger (for error logging)

Returns

projected points

Definition at line 208 of file utils.cpp.

                                                                                           {
  std::vector<ProjectedLaneletPoints> projected_points_per_lanelet;
 
  // loop over lanelets
  for (const auto& lanelet : lanelets) {
    ProjectedLaneletPoints projected_points;
 
    // project point to left bounds
    if (auto result = projectPointToLineStringAlongNormal(point, prev_point, next_point,
                                                          toEigen(lanelet.leftBound2d().basicLineString()))) {
      projected_points.left_bound_point = result->projected_point;
    } else {
      RCLCPP_WARN(logger, "Failed to project point (%.3f, %.3f) to left bounds of lanelet %ld", point.x(), point.y(),
                  lanelet.id());
    }
 
    // project point to centerline
    if (auto result = projectPointToLineStringAlongNormal(point, prev_point, next_point,
                                                          toEigen(lanelet.centerline2d().basicLineString()))) {
      projected_points.centerline_point = result->projected_point;
    } else {
      RCLCPP_WARN(logger, "Failed to project point (%.3f, %.3f) to centerline of lanelet %ld", point.x(), point.y(),
                  lanelet.id());
    }
 
    // project point to right bounds
    if (auto result = projectPointToLineStringAlongNormal(point, prev_point, next_point,
                                                          toEigen(lanelet.rightBound2d().basicLineString()))) {
      projected_points.right_bound_point = result->projected_point;
    } else {
      RCLCPP_WARN(logger, "Failed to project point (%.3f, %.3f) to right bounds of lanelet %ld", point.x(), point.y(),
                  lanelet.id());
    }
 
    projected_points_per_lanelet.push_back(projected_points);
  }
 
  return projected_points_per_lanelet;
}

projectPointToLineString() [1/2]

Eigen::Vector2d lanelet2_route_planning::projectPointToLineString	(	const Eigen::Vector2d &	point,
		const std::vector< Eigen::Vector2d > &	line_string )

Projects a point to the closest line segment of a line string.

Parameters

[in]	point	point
[in]	line_string	line string

Returns

projected point

Definition at line 231 of file geometry.cpp.

                                                                                                                {
  return lanelet::geometry::project(toLanelet(line_string), toLanelet(point));
}

projectPointToLineString() [2/2]

Eigen::Vector3d lanelet2_route_planning::projectPointToLineString	(	const Eigen::Vector3d &	point,
		const std::vector< Eigen::Vector3d > &	line_string )

Projects a 3D point to the closest line segment of a 3D line string.

Parameters

[in]	point	point
[in]	line_string	line string

Returns

projected point

Definition at line 235 of file geometry.cpp.

                                                                                                                {
  return lanelet::geometry::project(toLanelet(line_string), toLanelet(point));
}

projectPointToLineStringAlongAxis()

std::optional< ProjectPointToLineStringAlongAxisResult > lanelet2_route_planning::projectPointToLineStringAlongAxis	(	const Eigen::Vector2d &	point,
		const Eigen::Vector2d &	axis,
		const std::vector< Eigen::Vector2d > &	line_string )

Projects a point along an axis to the closest line segment of a line string.

Parameters

[in]	point	point
[in]	axis	axis direction
[in]	line_string	line string

Returns

projected point and whether intersection is on line segment

Definition at line 239 of file geometry.cpp.

                                                                                                        {
  ProjectPointToLineStringAlongAxisResult result;
  result.found_intersection_with_line_segment = false;
  double closest_distance_to_line_segment = std::numeric_limits<double>::max();
  bool found_at_least_one_intersection = false;
 
  // define straight line along axis at point
  std::vector<Eigen::Vector2d> axis_line = {point, point + axis};
 
  // loop over line segments
  for (size_t i = 0; i < line_string.size() - 1; ++i) {
    std::vector<Eigen::Vector2d> line_segment = {line_string[i], line_string[i + 1]};
 
    // find intersection of axis line and line segment
    if (auto inner_result = intersectionOfLines(axis_line, line_segment)) {
      const Eigen::Vector2d& intersection = inner_result->intersection;
      const bool intersects_line_segment = inner_result->intersects_line2;
      found_at_least_one_intersection = true;
      if (intersects_line_segment) {
        result.found_intersection_with_line_segment = true;
        result.projected_point = intersection;
        break;
      }
      double distance_to_line_segment =
          std::min((intersection - line_string[i]).norm(), (intersection - line_string[i + 1]).norm());
      if (distance_to_line_segment < closest_distance_to_line_segment) {
        closest_distance_to_line_segment = distance_to_line_segment;
        result.projected_point = intersection;
      }
    }
  }
 
  if (!found_at_least_one_intersection) {
    return std::nullopt;
  }
 
  return result;
}

projectPointToLineStringAlongNormal()

std::optional< ProjectPointToLineStringAlongAxisResult > lanelet2_route_planning::projectPointToLineStringAlongNormal	(	const Eigen::Vector2d &	point,
		const Eigen::Vector2d &	prev_point,
		const Eigen::Vector2d &	next_point,
		const std::vector< Eigen::Vector2d > &	line_string )

Projects a point to the closest line segment of a line string along the normal to the tangent at the point.

Parameters

[in]	point	point along line string
[in]	prev_point	previous point along line string
[in]	next_point	next point along line string
[in]	line_string	other line string to project to

Returns

projected point and whether intersection is on line segment

Definition at line 279 of file geometry.cpp.

                                                 {
  // find normal to tangent at point
  Eigen::Vector2d normal = normalOfPointAlongLineString(point, prev_point, next_point);
  if (normal == Eigen::Vector2d(0.0, 0.0)) {
    return std::nullopt;
  }
 
  // project current point to other line along normal to tangent
  if (auto result = projectPointToLineStringAlongAxis(point, normal, line_string)) {
    return result;
  }
  return std::nullopt;
}

regulatoryElementCancelLine()

std::optional< std::array< geometry_msgs::msg::Point, 2 > > lanelet2_route_planning::regulatoryElementCancelLine

(

const std::shared_ptr< const lanelet::RegulatoryElement > &

regulatory_element

)

Extracts the cancel line of a regulatory element.

Only the first cancel line of the regulatory element is considered. Only the end points of that cancel line are considered.

Parameters

[in]

regulatory_element

regulatory element

Returns

cancel line

Definition at line 574 of file utils.cpp.

                                                                             {
  const std::vector<lanelet::ConstLineString3d> cancel_lines =
      regulatory_element->getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::CancelLine);
  if (cancel_lines.empty()) {
    return std::nullopt;
  }
  const std::vector<Eigen::Vector3d> cancel_line = cancel_lines.front().basicLineString();
  if (cancel_line.size() < 2) {
    return std::nullopt;
  }
  std::array<geometry_msgs::msg::Point, 2> cancel_line_ros = {toRos(cancel_line.front()), toRos(cancel_line.back())};
  return cancel_line_ros;
}

regulatoryElementPositions()

std::vector< geometry_msgs::msg::Point > lanelet2_route_planning::regulatoryElementPositions

(

const std::shared_ptr< const lanelet::RegulatoryElement > &

regulatory_element

)

Extracts the sign/signal positions of a regulatory element.

Only the first point of referenced line strings is considered.

Parameters

[in]

regulatory_element

regulatory element

Returns

positions

Definition at line 589 of file utils.cpp.

                                                                             {
  std::vector<geometry_msgs::msg::Point> positions;
  const std::vector<lanelet::ConstLineString3d> sign_lines =
      regulatory_element->getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::Refers);
  for (const auto& const_sign_line : sign_lines) {
    const std::vector<Eigen::Vector3d> sign_line = const_sign_line.basicLineString();
    if (!sign_line.empty()) {
      positions.push_back(toRos(sign_line.front()));
    }
  }
  return positions;
}

regulatoryElementReferenceLine()

std::optional< std::array< geometry_msgs::msg::Point, 2 > > lanelet2_route_planning::regulatoryElementReferenceLine

(

const std::shared_ptr< const lanelet::RegulatoryElement > &

regulatory_element

)

Extracts the reference/effect line of a regulatory element.

Only the first reference line of the regulatory element is considered. Only the end points of that reference line are considered.

Parameters

[in]

regulatory_element

regulatory element

Returns

reference line

Definition at line 559 of file utils.cpp.

                                                                             {
  const std::vector<lanelet::ConstLineString3d> reference_lines =
      regulatory_element->getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::RefLine);
  if (reference_lines.empty()) {
    return std::nullopt;
  }
  const std::vector<Eigen::Vector3d> reference_line = reference_lines.front().basicLineString();
  if (reference_line.size() < 2) {
    return std::nullopt;
  }
  std::array<geometry_msgs::msg::Point, 2> reference_line_ros = {toRos(reference_line.front()), toRos(reference_line.back())};
  return reference_line_ros;
}

regulatoryElementSpeedLimit()

uint8_t lanelet2_route_planning::regulatoryElementSpeedLimit

(

const std::shared_ptr< const lanelet::RegulatoryElement > &

regulatory_element

)

Extracts the speed limit of a regulatory element of subtype 'speed_limit'.

As defined in route_planning_msgs::msg::RegulatoryElement.

Parameters

[in]

regulatory_element

regulatory element

Returns

speed limit [km/h]

Definition at line 631 of file utils.cpp.

                                                                                                             {
  uint8_t speed_limit = route_planning_msgs::msg::RegulatoryElement::META_VALUE_SPEED_UNKNOWN;
 
  // https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/RegulatoryElementTagging.md#speed-limit
  if (regulatory_element->hasAttribute("subtype")) {
    std::string subtype = regulatory_element->attribute("subtype").value();
    if (subtype == "speed_limit") {
      if (regulatory_element->hasAttribute("sign_type")) {
        std::string sign_type = regulatory_element->attribute("sign_type").value();
        std::smatch match;
        std::regex regex("(\\d+)\\s*(km/h|mph|mps)");
        if (std::regex_search(sign_type, match, regex)) {
          int speed = std::stoi(match.str(1));
          if (match.str(2) == "mph") {
            speed = static_cast<int>(speed * 1.60934);  // mph to km/h
          } else if (match.str(2) == "mps") {
            speed = static_cast<int>(speed * 3.6);  // mps to km/h
          }
          speed_limit = static_cast<uint8_t>(speed);
        } else {
          std::regex regex("(\\d+)");  // assume km/h if no unit is specified
          if (std::regex_search(sign_type, match, regex)) {
            speed_limit = static_cast<uint8_t>(std::stoi(match.str(1)));
          }
        }
      }
    }
  }
 
  uint8_t unlimited = route_planning_msgs::msg::RegulatoryElement::META_VALUE_SPEED_UNLIMITED;
  speed_limit = std::clamp(speed_limit, static_cast<uint8_t>(0), unlimited);
 
  return speed_limit;
}

regulatoryElementType()

std::pair< uint8_t, uint8_t > lanelet2_route_planning::regulatoryElementType

(

const std::shared_ptr< const lanelet::RegulatoryElement > &

regulatory_element

)

Extracts the type and meta value of a regulatory element.

https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/RegulatoryElementTagging.md

Type and meta value as in route_planning_msgs::msg::RegulatoryElement.

Parameters

[in]

regulatory_element

regulatory element

Returns

type and meta value

Definition at line 603 of file utils.cpp.

                                                                                                                         {
  uint8_t type = route_planning_msgs::msg::RegulatoryElement::TYPE_UNKNOWN;
  uint8_t meta_value = 0;
 
  // https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/RegulatoryElementTagging.md
  if (regulatory_element->hasAttribute("subtype")) {
    std::string subtype = regulatory_element->attribute("subtype").value();
    if (subtype == "traffic_light") {
      type = route_planning_msgs::msg::RegulatoryElement::TYPE_TRAFFIC_LIGHT;
    } else if (subtype == "speed_limit") {
      type = route_planning_msgs::msg::RegulatoryElement::TYPE_SPEED_LIMIT;
      meta_value = regulatoryElementSpeedLimit(regulatory_element);
    } else if (subtype == "right_of_way") {
      type = route_planning_msgs::msg::RegulatoryElement::TYPE_YIELD;
      const auto traffic_signs = regulatory_element->getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::Refers);
      const bool has_stop_sign = std::any_of(traffic_signs.begin(), traffic_signs.end(), [](const auto& traffic_sign) {
        return traffic_sign.hasAttribute("subtype") && traffic_sign.attribute("subtype").value() == "de206";
      });
      if (has_stop_sign) {
        type = route_planning_msgs::msg::RegulatoryElement::TYPE_STOP;
      }
    } else if (subtype == "all_way_stop") {
      type = route_planning_msgs::msg::RegulatoryElement::TYPE_STOP;
    }
  }
  return {type, meta_value};
}

resampleCenterlinesAlongPath()

ResampleCenterlinesAlongPathResult lanelet2_route_planning::resampleCenterlinesAlongPath	(	const lanelet::routing::LaneletPath &	path,
		const double	delta_s,
		bool	monotonically )

Equidistantly resamples lanelet centerlines along a path to one joint centerline.

Parameters

[in]	path	path
[in]	delta_s	sampling distance
[in]	monotonically	whether to ensure that the centerline cannot make turns greater than 90 degrees

Returns

resampled centerline and lanelet index by point

Definition at line 946 of file utils.cpp.

                                                                                    {
  // init variables
  ResampleCenterlinesAlongPathResult result;
  double resampling_offset = 0.0;
  Eigen::Vector2d prev_sampled_point = Eigen::Vector2d(0.0, 0.0);
  Eigen::Vector2d prev_sampled_point_orientation = Eigen::Vector2d(0.0, 0.0);
 
  // loop over lanelets in path
  for (size_t l = 0; l < path.size(); ++l) {
    // get centerline
    const lanelet::ConstLanelet& lanelet = path[l];
    lanelet::BasicLineString3d centerline = lanelet.centerline().basicLineString();
 
    // skip point if behind previous sampled point, e.g., if on adjacent lanelet in shortest path due to lane change
    if (monotonically && !result.centerline.empty()) {
      for (auto cit = centerline.begin(); cit != centerline.end();) {
        auto& centerline_point = *cit;
        if ((to2d(centerline_point) - prev_sampled_point).dot(prev_sampled_point_orientation) < 0) {  // angle > 90deg
          cit = centerline.erase(cit);
        } else {
          break;
        }
      }
    }
 
    // resample lanelet centerline
    std::vector<Eigen::Vector3d> resampled_centerline = resampleLineString(toEigen(centerline), delta_s, resampling_offset);
    result.centerline.insert(result.centerline.end(), resampled_centerline.begin(), resampled_centerline.end());
    result.lanelet_idx_by_point.insert(result.lanelet_idx_by_point.end(), resampled_centerline.size(), l);
 
    // update information for monotonicity check
    if (monotonically && !resampled_centerline.empty()) {
      if (resampled_centerline.size() > 1) {
        prev_sampled_point = to2d(resampled_centerline[resampled_centerline.size() - 2]);
      }
      const Eigen::Vector2d current_sampled_point = to2d(resampled_centerline.back());
      prev_sampled_point_orientation =
          tangentOfPointAlongLineString(current_sampled_point, prev_sampled_point, current_sampled_point);
      prev_sampled_point = current_sampled_point;
    }
  }
 
  return result;
}

resampleLineString()

std::vector< Eigen::Vector3d > lanelet2_route_planning::resampleLineString	(	const std::vector< Eigen::Vector3d > &	line_string,
		const double	delta,
		double &	offset )

Resamples a 3D line string with a constant sampling distance.

Sampling distance is measured in the xy-plane; z is interpolated.

Parameters

[in]	line	line string
[in]	delta	sampling distance
[in,out]	offset	starts sampling at offset distance, returns overshoot distance

Returns

resampled line string

Definition at line 152 of file geometry.cpp.

                                                                {
  std::vector<Eigen::Vector3d> resampled_line_string;
 
  // set initial sampling distance
  double sampling_distance = delta;
  if (offset == 0.0) {  // if no offset, start with first line point
    resampled_line_string.push_back(line_string.front());
  } else {  // else sample first point with offset != delta
    sampling_distance = offset;
  }
 
  // loop over all line segments
  for (size_t i = 1; i < line_string.size(); ++i) {
    // determine 2D segment length and 3D direction
    double segment_length = (to2d(line_string[i]) - to2d(line_string[i - 1])).norm();
    const Eigen::Vector3d segment_direction = (line_string[i] - line_string[i - 1]) / segment_length;
 
    // sample points along segment, increasing sampling_distance by delta
    while (segment_length >= sampling_distance) {
      const Eigen::Vector3d resampled_point = line_string[i - 1] + sampling_distance * segment_direction;
      resampled_line_string.push_back(resampled_point);
      sampling_distance += delta;
    }
 
    // reset sampling_distance for next segment, including overshoot
    sampling_distance = sampling_distance - segment_length;
  }
 
  // save overshoot in outgoing offset parameter
  offset = sampling_distance;
 
  return resampled_line_string;
}

sortLaneletsByMatchingCost()

void lanelet2_route_planning::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 )

Sorts lanelet candidates by their matching cost for a 2D point.

Points inside a lanelet are favored, lateral distance is penalized quadratically, and non-passable lanelets receive an additional penalty if traffic rules are available.

Derived simplified implementation from deprecated lanelet2_utilties package.

Parameters

[in]	point	point to match
[in,out]	lanelets_with_distances	candidate lanelets from lanelet::geometry::findNearest
[in]	traffic_rules	optional traffic rules used to penalize non-passable lanelets

Definition at line 1132 of file utils.cpp.

                                                                                                       {
  struct LaneletMatchingCost {
    std::pair<double, lanelet::ConstLanelet> lanelet_with_distance;
    double cost;
  };
 
  std::vector<LaneletMatchingCost> lanelet_matching_costs;
  lanelet_matching_costs.reserve(lanelets_with_distances.size());
 
  // compute the matching cost for each candidate lanelet
  for (const auto& lanelet_with_distance : lanelets_with_distances) {
    const lanelet::ConstLanelet& lanelet = lanelet_with_distance.second;
    const lanelet::ArcCoordinates arc_coordinates = lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), point);
    const bool point_is_inside_lanelet = lanelet::geometry::inside(lanelet, point);
 
    // favor lanelets containing the point and penalize lateral distance quadratically
    double cost = (point_is_inside_lanelet ? 0.5 : 2.0) * arc_coordinates.distance * arc_coordinates.distance;
 
    // keep non-passable lanelets as fallback candidates, but rank them lower
    if (traffic_rules && !traffic_rules.value()->canPass(lanelet)) {
      cost *= 2.0;
    }
 
    lanelet_matching_costs.push_back({lanelet_with_distance, cost});
  }
 
  // apply the sorted order to the original lanelet-distance pairs
  std::sort(lanelet_matching_costs.begin(), lanelet_matching_costs.end(),
            [](const LaneletMatchingCost& lhs, const LaneletMatchingCost& rhs) { return lhs.cost < rhs.cost; });
 
  for (size_t i = 0; i < lanelet_matching_costs.size(); ++i) {
    lanelets_with_distances.at(i) = lanelet_matching_costs[i].lanelet_with_distance;
  }
}

speedLimit() [1/2]

uint8_t lanelet2_route_planning::speedLimit	(	const lanelet::ConstLanelet &	lanelet,
		const bool	consider_regulatory_elements = true )

Extracts the speed limit of a lanelet.

Uses lanelet traffic rules to determine the speed limit for the whole lanelet.

If consider_regulatory_elements is true, speed-limit regulatory elements attached to the lanelet may override the lanelet-wide speed limit according to lanelet traffic rules. If consider_regulatory_elements is false, only the lanelet's own speed_limit and speed_limit_mandatory attributes are considered.

As defined in route_planning_msgs::msg::RegulatoryElement.

Parameters

[in]	lanelet	lanelet
[in]	consider_regulatory_elements	whether to consider regulatory elements related to the lanelet

Returns

speed limit [km/h]

Definition at line 704 of file utils.cpp.

                                                                                                {
  lanelet::traffic_rules::TrafficRulesPtr traffic_rules = getTrafficRules();
  lanelet::traffic_rules::SpeedLimitInformation speed_limit_info;
  if (consider_regulatory_elements) {
    speed_limit_info = traffic_rules->speedLimit(lanelet);
  } else {
    uint8_t unlimited = route_planning_msgs::msg::RegulatoryElement::META_VALUE_SPEED_UNLIMITED;
    if (!lanelet.hasAttribute(lanelet::AttributeName::SpeedLimit)) {
      return unlimited;
    }
    auto speed_limit = lanelet.attribute(lanelet::AttributeName::SpeedLimit).asVelocity();
    if (!speed_limit) {
      return unlimited;
    }
    bool is_mandatory = true;
    const std::string speed_limit_mandatory_attribute =
        lanelet::AttributeNamesString::SpeedLimitMandatory;  // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay)
    if (lanelet.hasAttribute(speed_limit_mandatory_attribute)) {
      is_mandatory = lanelet.attribute(speed_limit_mandatory_attribute).value() != std::string("no");
    }
    speed_limit_info = lanelet::traffic_rules::SpeedLimitInformation{*speed_limit, is_mandatory};
  }
  uint8_t unlimited = route_planning_msgs::msg::RegulatoryElement::META_VALUE_SPEED_UNLIMITED;
  if (speed_limit_info.isMandatory) {
    int speed_limit = static_cast<int>(std::round(lanelet::units::KmHQuantity(speed_limit_info.speedLimit).value()));
    speed_limit = std::clamp(speed_limit, 0, static_cast<int>(unlimited));
    return static_cast<uint8_t>(speed_limit);
  }
  return unlimited;
}

speedLimit() [2/2]

uint8_t lanelet2_route_planning::speedLimit	(	const lanelet::ConstLanelet &	lanelet,
		const Eigen::Vector2d &	point )

Extracts the speed limit of a lanelet at a specific position on that lanelet.

Speed-limit regulatory elements take precedence over the lanelet-wide speed limit, but only once the given point is behind the regulatory element's reference line and, if present, still before its cancel line.

If no applicable speed-limit regulatory element is active at the given point, this falls back to the lanelet-wide speed limit. If the point is still before an intersecting reference line, the fallback ignores regulatory elements and uses only the lanelet's own speed-limit attributes.

This implementation considers only the first reference line and first cancel line of a speed-limit regulatory element, using their end points.

As defined in route_planning_msgs::msg::RegulatoryElement.

Parameters

[in]	lanelet	lanelet
[in]	point	point on or near the lanelet

Returns

speed limit [km/h]

Definition at line 735 of file utils.cpp.

                                                                                   {
  uint8_t speed_limit = route_planning_msgs::msg::RegulatoryElement::META_VALUE_SPEED_UNLIMITED;
  std::vector<Eigen::Vector2d> centerline = toEigen(lanelet.centerline2d().basicLineString());
  double best_reference_arc_length = 0.0;
  bool found_valid_regulatory_element_speed_limit = false;
  bool found_future_reference_line = false;
  bool found_past_cancel_line = false;
 
  // loop over regulatory elements of lanelet
  const auto regulatory_elements = lanelet.regulatoryElements();
  for (const auto& regulatory_element : regulatory_elements) {
    // extract type and meta value, skip non-speed-limit regulatory elements
    uint8_t regulatory_element_type = route_planning_msgs::msg::RegulatoryElement::TYPE_UNKNOWN;
    uint8_t regulatory_element_meta_value = route_planning_msgs::msg::RegulatoryElement::META_VALUE_UNKNOWN;
    std::tie(regulatory_element_type, regulatory_element_meta_value) = regulatoryElementType(regulatory_element);
    if (regulatory_element_type != route_planning_msgs::msg::RegulatoryElement::TYPE_SPEED_LIMIT) {
      continue;
    }
 
    // extract reference line
    if (auto reference_line = regulatoryElementReferenceLine(regulatory_element)) {
      std::vector<Eigen::Vector2d> reference_line_2d = {toEigen2d(reference_line->at(0)), toEigen2d(reference_line->at(1))};
 
      // check if reference line intersects lanelet centerline
      bool reference_line_intersects_centerline = false;
      double reference_arc_length = 0.0;
      for (size_t i = 0; i < centerline.size() - 1; ++i) {
        std::vector<Eigen::Vector2d> centerline_segment = {centerline[i], centerline[i + 1]};
        if (auto result = intersectionOfLines(reference_line_2d, centerline_segment)) {
          if (result->intersects_line1 && result->intersects_line2) {
            reference_arc_length =
                lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), toLanelet(result->intersection)).length;
            reference_line_intersects_centerline = true;
            break;
          }
        }
      }
 
      // check if cancel line intersects lanelet centerline
      bool cancel_line_intersects_centerline = false;
      double cancel_arc_length = 0.0;
      if (auto cancel_line = regulatoryElementCancelLine(regulatory_element)) {
        std::vector<Eigen::Vector2d> cancel_line_2d = {toEigen2d(cancel_line->at(0)), toEigen2d(cancel_line->at(1))};
        for (size_t i = 0; i < centerline.size() - 1; ++i) {
          std::vector<Eigen::Vector2d> centerline_segment = {centerline[i], centerline[i + 1]};
          if (auto result = intersectionOfLines(cancel_line_2d, centerline_segment)) {
            if (result->intersects_line1 && result->intersects_line2) {
              cancel_arc_length =
                  lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), toLanelet(result->intersection)).length;
              cancel_line_intersects_centerline = true;
              break;
            }
          }
        }
      }
 
      // if given point is behind reference line and before cancel line, use speed limit of regulatory element
      double point_arc_length = lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), toLanelet(point)).length;
      bool point_is_behind_reference_line = (!reference_line_intersects_centerline || (point_arc_length >= reference_arc_length));
      bool point_is_before_cancel_line = (!cancel_line_intersects_centerline || (point_arc_length < cancel_arc_length));
      if (!point_is_behind_reference_line) {
        found_future_reference_line = true;
      }
      if (!point_is_before_cancel_line) {
        found_past_cancel_line = true;
      }
      if (point_is_behind_reference_line && point_is_before_cancel_line) {
        if (!found_valid_regulatory_element_speed_limit || reference_arc_length > best_reference_arc_length) {
          // only update speed limit if regulatory element is ahead of previously found regulatory element
          // (in case of multiple speed limit regulatory elements on lanelet)
          speed_limit = regulatory_element_meta_value;
          best_reference_arc_length = reference_arc_length;
          found_valid_regulatory_element_speed_limit = true;
        }
      }
 
    } else {
      continue;
    }
  }
 
  // if no valid regulatory element speed limit is found, fall back to lanelet speed limit;
  // that function will still yield regulatory-element-based speed limits, if related to the lanelet
  // even though there are no intersecting reference lines
  if (!found_valid_regulatory_element_speed_limit) {
    if (found_future_reference_line || found_past_cancel_line) {
      speed_limit = speedLimit(lanelet, false);
    } else {
      speed_limit = speedLimit(lanelet, true);
    }
  }
 
  speed_limit =
      std::clamp(speed_limit, static_cast<uint8_t>(0), route_planning_msgs::msg::RegulatoryElement::META_VALUE_SPEED_UNLIMITED);
 
  return speed_limit;
}

suggestedReferenceLineToEigen()

std::vector< Eigen::Vector3d > lanelet2_route_planning::suggestedReferenceLineToEigen

(

const std::vector< route_planning_msgs::msg::RouteElement > &

route_elements

)

Extracts the reference line along the suggested lane from a list of RouteElements.

Parameters

[in]

route_elements

list of RouteElements

Returns

reference line

Definition at line 97 of file conversions.cpp.

                                                                         {
  std::vector<Eigen::Vector3d> reference_line;
  for (const auto& route_element : route_elements) {
    const auto& lane_element = route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
    reference_line.push_back(toEigen(lane_element.reference_pose.position));
  }
  return reference_line;
}

suggestedTurnSignal()

std::tuple< uint8_t, int > lanelet2_route_planning::suggestedTurnSignal	(	const lanelet::ConstLanelet &	lanelet,
		const rclcpp::Logger &	logger )

Extracts the suggested turn signal of a lanelet.

Checks for the attribute suggested_turn_signal with values left, right, or hazard. Checks for the attribute suggested_turn_signal_distance_ahead to get the suggested distance ahead for the turn signal.

Parameters

[in]	lanelet	lanelet
[in]	logger	logger (for error logging)

Returns

suggested turn signal and distance ahead

Definition at line 833 of file utils.cpp.

                                                                                                           {
  uint8_t suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE;
  int suggested_turn_signal_distance_ahead = -1;
 
  // parse suggested turn signal attribute
  if (lanelet.hasAttribute("suggested_turn_signal")) {
    std::string suggested_turn_signal_str = lanelet.attribute("suggested_turn_signal").value();
    if (suggested_turn_signal_str == "left") {
      suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT;
    } else if (suggested_turn_signal_str == "right") {
      suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT;
    } else if (suggested_turn_signal_str == "hazard") {
      suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_HAZARD;
    } else {
      suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE;
      RCLCPP_ERROR(logger, "Could not parse 'suggested_turn_signal' attribute value of lanelet '%ld': '%s'", lanelet.id(),
                   suggested_turn_signal_str.c_str());
      return std::make_tuple(suggested_turn_signal, suggested_turn_signal_distance_ahead);
    }
 
    // parse suggested turn signal distance ahead attribute
    if (lanelet.hasAttribute("suggested_turn_signal_distance_ahead")) {
      std::string suggested_turn_signal_distance_ahead_str = lanelet.attribute("suggested_turn_signal_distance_ahead").value();
      try {
        suggested_turn_signal_distance_ahead = std::stoi(suggested_turn_signal_distance_ahead_str);
      } catch (const std::exception&) {
        RCLCPP_ERROR(logger, "Could not parse 'suggested_turn_signal_distance_ahead' attribute value of lanelet '%ld': '%s'",
                     lanelet.id(), suggested_turn_signal_distance_ahead_str.c_str());
        return std::make_tuple(suggested_turn_signal, suggested_turn_signal_distance_ahead);
      }
      if (suggested_turn_signal_distance_ahead < 0) {
        RCLCPP_ERROR(logger,
                     "'suggested_turn_signal_distance_ahead' attribute value of lanelet '%ld' is negative, clamping to 0: %d",
                     lanelet.id(), suggested_turn_signal_distance_ahead);
        suggested_turn_signal_distance_ahead = 0;
      }
    } else {
      RCLCPP_ERROR(logger,
                   "Lanelet '%ld' has 'suggested_turn_signal' attribute but no 'suggested_turn_signal_distance_ahead', ignoring "
                   "suggested turn signal",
                   lanelet.id());
    }
  }
  return std::make_tuple(suggested_turn_signal, suggested_turn_signal_distance_ahead);
}

tangentOfPointAlongLineString()

Eigen::Vector2d lanelet2_route_planning::tangentOfPointAlongLineString	(	const Eigen::Vector2d &	point,
		const Eigen::Vector2d &	prev_point,
		const Eigen::Vector2d &	next_point )

Computes a unit vector tangential to a point along a line string.

The tangent is pointing in the average direction of previous to current and current to next point directions.

If two points are equal, the tangent is pointing in the direction from/to the third. If three points are equal, the tangent is undefined and returns (0,0).

Parameters

[in]	point	point along line string
[in]	prev_point	previous point along line string
[in]	next_point	next point along line string

Returns

unit tangential vector

Definition at line 123 of file geometry.cpp.

                                                                               {
  Eigen::Vector2d tangent;
 
  if (point == prev_point && point != next_point) {  // single line segment
    tangent = (next_point - point).normalized();
  } else if (point != prev_point && point == next_point) {  // single line segment
    tangent = (point - prev_point).normalized();
  } else if (point == prev_point && point == next_point) {  // single point
    tangent = Eigen::Vector2d(0.0, 0.0);
  } else {  // proper two line segments with previous and next point
    const Eigen::Vector2d prev_to_point_unit = (point - prev_point).normalized();
    const Eigen::Vector2d point_to_next_unit = (next_point - point).normalized();
    tangent = (prev_to_point_unit + point_to_next_unit).normalized();
  }
 
  return tangent;
}

to2d() [1/2]

Eigen::Vector2d lanelet2_route_planning::to2d

(

const Eigen::Vector3d &

point

)

Converts a 3D Eigen point to a 2D Eigen point.

Removes the z-coordinate.

Parameters

[in]

point

point

Returns

converted point

Definition at line 11 of file conversions.cpp.

{ return point.head<2>(); }

to2d() [2/2]

std::vector< Eigen::Vector2d > lanelet2_route_planning::to2d

(

const std::vector< Eigen::Vector3d > &

points

)

Converts a vector of 3D Eigen points to a vector of 2D Eigen points.

Removes the z-coordinate.

Parameters

[in]

points

points

Returns

converted points

Definition at line 17 of file conversions.cpp.

                                                                      {
  std::vector<Eigen::Vector2d> points_2d;
  for (const auto& point : points) {
    points_2d.push_back(to2d(point));
  }
  return points_2d;
}

to3d() [1/3]

Eigen::Vector3d lanelet2_route_planning::to3d

(

const Eigen::Vector2d &

point

)

Converts a 2D Eigen point to a 3D Eigen point.

Adds a z-coordinate of 0.

Parameters

[in]

point

point

Returns

converted point

Definition at line 13 of file conversions.cpp.

{ return Eigen::Vector3d(point.x(), point.y(), 0.0); }

to3d() [2/3]

Eigen::Vector3d lanelet2_route_planning::to3d	(	const Eigen::Vector2d &	point,
		double	z )

Converts a 2D Eigen point to a 3D Eigen point.

Adds the given z-coordinate.

Parameters

[in]	point	point
[in]	z	z-coordinate

Returns

converted point

Definition at line 15 of file conversions.cpp.

{ return Eigen::Vector3d(point.x(), point.y(), z); }

to3d() [3/3]

std::vector< Eigen::Vector3d > lanelet2_route_planning::to3d

(

const std::vector< Eigen::Vector2d > &

points

)

Converts a vector of 2D Eigen points to a vector of 3D Eigen points.

Adds a z-coordinate of 0.

Parameters

[in]

points

points

Returns

converted points

Definition at line 25 of file conversions.cpp.

                                                                      {
  std::vector<Eigen::Vector3d> points_3d;
  for (const auto& point : points) {
    points_3d.push_back(to3d(point));
  }
  return points_3d;
}

toEigen() [1/3]

Eigen::Vector3d lanelet2_route_planning::toEigen

(

const geometry_msgs::msg::Point &

point

)

Converts a ROS point to a 3D Eigen point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 51 of file conversions.cpp.

{ return Eigen::Vector3d(point.x, point.y, point.z); }

toEigen() [2/3]

std::vector< Eigen::Vector2d > lanelet2_route_planning::toEigen

(

const lanelet::BasicLineString2d &

line_string

)

Converts a 2D Lanelet line string to a vector of Eigen points.

Parameters

[in]

line_string

line string

Returns

converted line string

Definition at line 61 of file conversions.cpp.

                                                                              {
  return std::vector<Eigen::Vector2d>(line_string.begin(), line_string.end());
}

toEigen() [3/3]

std::vector< Eigen::Vector3d > lanelet2_route_planning::toEigen

(

const lanelet::BasicLineString3d &

line_string

)

Converts a 3D Lanelet line string to a vector of Eigen points.

Parameters

[in]

line_string

line string

Returns

converted line string

Definition at line 65 of file conversions.cpp.

                                                                              {
  return std::vector<Eigen::Vector3d>(line_string.begin(), line_string.end());
}

toEigen2d()

Eigen::Vector2d lanelet2_route_planning::toEigen2d

(

const geometry_msgs::msg::Point &

point

)

Converts a ROS point to a 2D Eigen point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 49 of file conversions.cpp.

{ return Eigen::Vector2d(point.x, point.y); }

toLanelet() [1/4]

lanelet::BasicPoint2d lanelet2_route_planning::toLanelet

(

const Eigen::Vector2d &

point

)

Converts a 2D Eigen point to a Lanelet point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 45 of file conversions.cpp.

{ return lanelet::BasicPoint2d(point.x(), point.y()); }

toLanelet() [2/4]

lanelet::BasicPoint3d lanelet2_route_planning::toLanelet

(

const Eigen::Vector3d &

point

)

Converts a 3D Eigen point to a Lanelet point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 47 of file conversions.cpp.

{ return point; }

toLanelet() [3/4]

lanelet::BasicLineString2d lanelet2_route_planning::toLanelet

(

const std::vector< Eigen::Vector2d > &

line_string

)

Converts a vector of 2D Eigen points to a Lanelet line string.

Parameters

[in]

line_string

line string

Returns

converted line string

Definition at line 69 of file conversions.cpp.

                                                                                {
  lanelet::BasicLineString2d lanelet_line_string;
  for (const auto& point : line_string) {
    lanelet_line_string.push_back(toLanelet(point));
  }
  return lanelet_line_string;
}

toLanelet() [4/4]

lanelet::BasicLineString3d lanelet2_route_planning::toLanelet

(

const std::vector< Eigen::Vector3d > &

line_string

)

Converts a vector of 3D Eigen points to a Lanelet line string.

Parameters

[in]

line_string

line string

Returns

converted line string

Definition at line 77 of file conversions.cpp.

                                                                                {
  lanelet::BasicLineString3d lanelet_line_string;
  for (const auto& point : line_string) {
    lanelet_line_string.push_back(toLanelet(point));
  }
  return lanelet_line_string;
}

toRos() [1/3]

geometry_msgs::msg::Point lanelet2_route_planning::toRos

(

const Eigen::Vector2d &

point

)

Converts a 2D Eigen point to a ROS point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 33 of file conversions.cpp.

{ return toRos(to3d(point)); }

toRos() [2/3]

geometry_msgs::msg::Point lanelet2_route_planning::toRos

(

const Eigen::Vector3d &

point

)

Converts a 3D Eigen point to a ROS point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 35 of file conversions.cpp.

                                                        {
  geometry_msgs::msg::Point ros_point;
  ros_point.x = point.x();
  ros_point.y = point.y();
  ros_point.z = point.z();
  return ros_point;
}

toRos() [3/3]

geometry_msgs::msg::Point lanelet2_route_planning::toRos

(

const lanelet::BasicPoint2d &

point

)

Converts a 2D Lanelet point to a ROS point.

Parameters

[in]

point

point

Returns

converted point

Definition at line 43 of file conversions.cpp.

{ return toRos(Eigen::Vector2d(point.x(), point.y())); }

toRosQuaternion()

geometry_msgs::msg::Quaternion lanelet2_route_planning::toRosQuaternion

(

const Eigen::Vector2d &

vector

)

Converts a 2D Eigen vector pointing in a specific direction to a ROS quaternion.

The vector is assumed to lie in the xy-plane.

Parameters

[in]

vector

vector

Returns

converted quaternion

Definition at line 85 of file conversions.cpp.

                                                                        {
  Eigen::Vector2d unit_vector = vector.normalized();
  double angle = std::atan2(unit_vector.y(), unit_vector.x());
  Eigen::Quaterniond quaternion(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()));
  geometry_msgs::msg::Quaternion ros_quaternion;
  ros_quaternion.x = quaternion.x();
  ros_quaternion.y = quaternion.y();
  ros_quaternion.z = quaternion.z();
  ros_quaternion.w = quaternion.w();
  return ros_quaternion;
}

wrapAngle()

double lanelet2_route_planning::wrapAngle

(

const double

angle

)

Wraps an angle to the range [-π, π].

Parameters

[in]

angle

angle to wrap

Returns

wrapped angle [radians]

Definition at line 73 of file geometry.cpp.

                                     {
  double wrapped_angle = angle;
  while (wrapped_angle > M_PI) wrapped_angle -= 2 * M_PI;
  while (wrapped_angle < -M_PI) wrapped_angle += 2 * M_PI;
  return wrapped_angle;
}

Variable Documentation

is_vector_v

template<typename C >

bool lanelet2_route_planning::is_vector_v = is_vector<C>::value

inlineconstexpr

Definition at line 34 of file lanelet2_route_planning.hpp.