traffic_simulator::lanelet_wrapper::pose Namespace Reference

Functions
auto	toMapPose (const LaneletPose &lanelet_pose, const bool fill_pitch=true) -> PoseStamped
auto	isAltitudeMatching (const double current_altitude, const double target_altitude) -> bool
auto	toLaneletPose (const Pose &map_pose, const lanelet::Id lanelet_id, const double matching_distance=1.0) -> std::optional< LaneletPose >
auto	toLaneletPose (const Pose &map_pose, const lanelet::Ids &lanelet_ids, const double matching_distance=1.0) -> std::optional< LaneletPose >
auto	toLaneletPose (const Pose &map_pose, const bool include_crosswalk, const double matching_distance=1.0) -> std::optional< LaneletPose >
auto	toLaneletPose (const Pose &map_pose, const BoundingBox &bounding_box, const bool include_crosswalk, const double matching_distance=1.0, const RoutingGraphType type=RoutingConfiguration().routing_graph_type) -> std::optional< LaneletPose >
auto	toLaneletPoses (const Pose &map_pose, const lanelet::Id lanelet_id, const double matching_distance=5.0, const bool include_opposite_direction=true, const RoutingGraphType type=RoutingConfiguration().routing_graph_type) -> std::vector< LaneletPose >
auto	alternativeLaneletPoses (const LaneletPose &reference_lanelet_pose) -> std::vector< LaneletPose >
	Retrieves alternative lanelet poses based on the reference lanelet pose. More...
auto	alongLaneletPose (const LaneletPose &from_pose, const lanelet::Ids &route_lanelets, const double distance) -> LaneletPose
auto	alongLaneletPose (const LaneletPose &from_pose, const double distance, const RoutingGraphType type=RoutingConfiguration().routing_graph_type) -> LaneletPose
auto	canonicalizeLaneletPose (const LaneletPose &lanelet_pose) -> std::tuple< std::optional< LaneletPose >, std::optional< lanelet::Id >>
	Canonicalizes a given LaneletPose by adjusting the longitudinal position (s) to ensure it lies within the bounds of the specified lanelet. If the position is out of bounds, it traverses to previous or next lanelets to find the canonicalized position. More...
auto	canonicalizeLaneletPose (const LaneletPose &lanelet_pose, const lanelet::Ids &route_lanelets) -> std::tuple< std::optional< LaneletPose >, std::optional< lanelet::Id >>
auto	findMatchingLanes (const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, const bool include_crosswalk, const double matching_distance=1.0, const double reduction_ratio=DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) -> std::optional< std::set< std::pair< double, lanelet::Id >>>
auto	matchToLane (const Pose &map_pose, const BoundingBox &bounding_box, const bool include_crosswalk, const double matching_distance=1.0, const double reduction_ratio=DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, const RoutingGraphType type=RoutingConfiguration().routing_graph_type) -> std::optional< lanelet::Id >
auto	leftLaneletIds (const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) -> lanelet::Ids
auto	rightLaneletIds (const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) -> lanelet::Ids

Function Documentation

alongLaneletPose() [1/2]

auto traffic_simulator::lanelet_wrapper::pose::alongLaneletPose	(	const LaneletPose &	from_pose,
		const double	distance,
		const RoutingGraphType	type = RoutingConfiguration().routing_graph_type
	)		-> LaneletPose

Note

if empty try to use other than "straight", but the first found

if empty try to use other than "straight", but the first found

if empty try to use other than "straight", but the first found

if empty try to use other than "straight", but the first found

alongLaneletPose() [2/2]

auto traffic_simulator::lanelet_wrapper::pose::alongLaneletPose	(	const LaneletPose &	from_pose,
		const lanelet::Ids &	route_lanelets,
		const double	distance
	)		-> LaneletPose

Note

If canonicalize succeed, just return canonicalized pose

If canonicalize failed, return lanelet pose as end of road

If canonicalize succeed, just return canonicalized pose

If canonicalize failed, return lanelet pose as end of road

alternativeLaneletPoses()

auto traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses

(

const LaneletPose &

reference_lanelet_pose

)

-> std::vector<LaneletPose>

Retrieves alternative lanelet poses based on the reference lanelet pose.

This method computes alternative lanelet poses in the previous and next lanelets relative to a given reference lanelet pose. It recursively explores the neighboring lanelets until no further alternatives are found. The decision of whether a pose belongs to a previous or next lanelet is based on the s value of the reference pose:

• If s is negative, the pose is assumed to be on the previous lanelet.
• If s exceeds the lanelet length, the pose is assumed to be on the next lanelet.
• If s is within the valid range of the lanelet (from 0 to the lanelet's length), the reference lanelet pose is returned directly.

Parameters

reference_lanelet_pose

The reference pose on a lanelet, used to determine its position and compute alternatives in neighboring lanelets.

Returns

A vector of alternative LaneletPose objects representing poses in the neighboring lanelets, or the reference pose if no alternatives are found.

Note

If s value under 0, it means this pose is on the previous lanelet.

If s value overs it's lanelet length, it means this pose is on the next lanelet.

If s value is in range [0,length_of_the_lanelet], return lanelet_pose.

If s value under 0, it means this pose is on the previous lanelet.

If s value overs it's lanelet length, it means this pose is on the next lanelet.

If s value is in range [0,length_of_the_lanelet], return lanelet_pose.

canonicalizeLaneletPose() [1/2]

auto traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose

(

const LaneletPose &

lanelet_pose

)

-> std::tuple< std::optional< LaneletPose >, std::optional< lanelet::Id >>

Canonicalizes a given LaneletPose by adjusting the longitudinal position (s) to ensure it lies within the bounds of the specified lanelet. If the position is out of bounds, it traverses to previous or next lanelets to find the canonicalized position.

If the provided pose has a longitudinal position (s) less than 0, the function traverses to previous lanelets until a valid position is found or no previous lanelets exist.

If the longitudinal position (s) exceeds the length of the current lanelet, the function traverses to next lanelets until the position is valid or no next lanelets exist.

Parameters

lanelet_pose

The input LaneletPose to canonicalize, containing a lanelet ID and longitudinal position (s).

Returns

A tuple where:

• The first element is an optional canonicalized LaneletPose (std::nullopt if canonicalization fails).
• The second element is an optional lanelet ID where the process stopped (std::nullopt if successful).

canonicalizeLaneletPose() [2/2]

auto traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose	(	const LaneletPose &	lanelet_pose,
		const lanelet::Ids &	route_lanelets
	)		-> std::tuple<std::optional<LaneletPose>, std::optional<lanelet::Id>>

Note

When canonicalizing to backward lanelet_id, do not consider route

When canonicalizing to forward lanelet_id, consider route

When canonicalizing to backward lanelet_id, do not consider route

When canonicalizing to forward lanelet_id, consider route

findMatchingLanes()

auto traffic_simulator::lanelet_wrapper::pose::findMatchingLanes	(	const geometry_msgs::msg::Pose &	map_pose,
		const traffic_simulator_msgs::msg::BoundingBox &	bounding_box,
		const bool	include_crosswalk,
		const double	matching_distance = 1.0,
		const double	reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO,
		const traffic_simulator::RoutingGraphType	type = traffic_simulator::RoutingConfiguration().routing_graph_type
	)		-> std::optional<std::set<std::pair<double, lanelet::Id>>>

isAltitudeMatching()

auto traffic_simulator::lanelet_wrapper::pose::isAltitudeMatching	(	const double	current_altitude,
		const double	target_altitude
	)		-> bool

Justification for using a fixed altitude_threshold value of 1.0 [m].

Using a fixed altitude_threshold value of 1.0 [m] is justified because the entity's Z-position is always relative to its base. This eliminates the need to dynamically adjust the threshold based on the entity's dimensions, ensuring consistent altitude matching regardless of the entity type.

The position of the entity is defined relative to its base, typically the center of the rear axle projected onto the ground in the case of vehicles.

Note

There is no technical basis for this value; it was determined based on experiments.

Justification for using a fixed altitude_threshold value of 1.0 [m].

Using a fixed altitude_threshold value of 1.0 [m] is justified because the entity's Z-position is always relative to its base. This eliminates the need to dynamically adjust the threshold based on the entity's dimensions, ensuring consistent altitude matching regardless of the entity type.

The position of the entity is defined relative to its base, typically the center of the rear axle projected onto the ground in the case of vehicles.

Note

There is no technical basis for this value; it was determined based on experiments.

leftLaneletIds()

auto traffic_simulator::lanelet_wrapper::pose::leftLaneletIds	(	const lanelet::Id	lanelet_id,
		const RoutingGraphType	type,
		const bool	include_opposite_direction
	)		-> lanelet::Ids

matchToLane()

auto traffic_simulator::lanelet_wrapper::pose::matchToLane	(	const Pose &	map_pose,
		const BoundingBox &	bounding_box,
		const bool	include_crosswalk,
		const double	matching_distance = 1.0,
		const double	reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO,
		const RoutingGraphType	type = RoutingConfiguration().routing_graph_type
	)		-> std::optional<lanelet::Id>

Note

findMatchingLanes returns a container sorted by distance - increasing, return the nearest id

findMatchingLanes returns a container sorted by distance - increasing, return the nearest id

rightLaneletIds()

auto traffic_simulator::lanelet_wrapper::pose::rightLaneletIds	(	const lanelet::Id	lanelet_id,
		const RoutingGraphType	type,
		const bool	include_opposite_direction
	)		-> lanelet::Ids

toLaneletPose() [1/4]

auto traffic_simulator::lanelet_wrapper::pose::toLaneletPose	(	const Pose &	map_pose,
		const bool	include_crosswalk,
		const double	matching_distance = 1.0
	)		-> std::optional<LaneletPose>

Note

Hardcoded parameter, this value has no technical basis and is determined based on experimentation.

Todo:

Add doxygen comments as soon as you know the meaning and rationale of the value.

Note

Hardcoded parameter, this value has no technical basis and is determined based on experimentation.

Todo:

Add doxygen comments as soon as you know the meaning and rationale of the value.

toLaneletPose() [2/4]

auto traffic_simulator::lanelet_wrapper::pose::toLaneletPose	(	const Pose &	map_pose,
		const BoundingBox &	bounding_box,
		const bool	include_crosswalk,
		const double	matching_distance = 1.0,
		const RoutingGraphType	type = RoutingConfiguration().routing_graph_type
	)		-> std::optional<LaneletPose>

toLaneletPose() [3/4]

auto traffic_simulator::lanelet_wrapper::pose::toLaneletPose	(	const Pose &	map_pose,
		const lanelet::Id	lanelet_id,
		const double	matching_distance = 1.0
	)		-> std::optional<LaneletPose>

Note

yaw_threshold_deg is used to determine whether the entity is going straight, it defines the maximum allowed rotation with respect to the lanelet centerline.

yaw_threshold_deg is used to determine whether the entity is going straight, it defines the maximum allowed rotation with respect to the lanelet centerline.

toLaneletPose() [4/4]

auto traffic_simulator::lanelet_wrapper::pose::toLaneletPose	(	const Pose &	map_pose,
		const lanelet::Ids &	lanelet_ids,
		const double	matching_distance = 1.0
	)		-> std::optional<LaneletPose>

toLaneletPoses()

auto traffic_simulator::lanelet_wrapper::pose::toLaneletPoses	(	const Pose &	map_pose,
		const lanelet::Id	lanelet_id,
		const double	matching_distance = 5.0,
		const bool	include_opposite_direction = true,
		const RoutingGraphType	type = RoutingConfiguration().routing_graph_type
	)		-> std::vector<LaneletPose>

toMapPose()

auto traffic_simulator::lanelet_wrapper::pose::toMapPose	(	const LaneletPose &	lanelet_pose,
		const bool	fill_pitch = true
	)		-> PoseStamped

Note

map position

map orientation

map position

map orientation