Classes
struct	ControllerError

struct	PredictedState

class	FollowWaypointController

Functions
auto	makeUpdatedStatus (const traffic_simulator_msgs::msg::EntityStatus &entity_status, traffic_simulator_msgs::msg::PolylineTrajectory &polyline_trajectory, const traffic_simulator_msgs::msg::BehaviorParameter &behavior_parameter, const double step_time, const double matching_distance, std::optional< double > target_speed) -> std::optional< EntityStatus >
	Computes entity state after one simulation step along a polyline trajectory. More...

Function Documentation

◆ makeUpdatedStatus()

auto traffic_simulator::follow_trajectory::makeUpdatedStatus	(	const traffic_simulator_msgs::msg::EntityStatus &	entity_status,
		traffic_simulator_msgs::msg::PolylineTrajectory &	polyline_trajectory,
		const traffic_simulator_msgs::msg::BehaviorParameter &	behavior_parameter,
		const double	step_time,
		const double	matching_distance,
		std::optional< double >	target_speed
	)		-> std::optional< EntityStatus >

Computes entity state after one simulation step along a polyline trajectory.

Validates entity state and trajectory, computes desired velocity respecting dynamic constraints (acceleration, deceleration, jerk limits), updates position/orientation/velocity, and advances to next waypoint when reached (recursive). Handles both timed and untimed waypoints.

Returns: EntityStatus with updated state, or nullopt when final waypoint reached and stopped

Exceptions

common::Error Entity/trajectory has NaN/infinity values, acceleration constraints violated, arrival time missed, final waypoint unreachable (stopped too far from target), or intermediate waypoint arrival time reached but entity has not yet reached waypoint position (impossible timing)

FINAL WAYPOINT CASES: ┌──────────────────────────────┬─────────────────────────────────┬──────────────────────┐ │ Entity State │ Timing Condition │ Return │ ├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤ │ Distance ≤ threshold │ No arrival time specified │ nullopt │ │ Velocity = 0 │ (remaining_time = NaN) │ (trajectory done) │ │ Acceleration = 0 │ │ │ ├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤ │ Distance ≤ threshold │ Arrival time reached │ nullopt │ │ Velocity = 0 │ (remaining_time < step_time/2) │ (trajectory done) │ │ Acceleration = 0 │ │ │ ├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤ │ Distance ≤ threshold │ Arrival time not reached │ EntityStatus │ │ Velocity = 0 │ (remaining_time ≥ step_time/2) │ velocity = 0 (wait) │ │ Acceleration = 0 │ │ │ ├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤ │ Distance > threshold │ Any │ throw │ │ Velocity = 0 │ │ Error │ │ Acceleration = 0 │ │ │ ├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤ │ Distance ≤ threshold │ Any │ EntityStatus │ │ Velocity > 0 │ │ brake to stop │ ├──────────────────────────────┼─────────────────────────────────┼──────────────────────┤ │ Distance > threshold │ Any │ EntityStatus │ │ Velocity > 0 │ │ move toward waypoint │ └──────────────────────────────┴─────────────────────────────────┴──────────────────────┘

INTERMEDIATE WAYPOINT CASES: ┌───────────────────────────────────┬──────────────────────────────┬──────────────────────┐ │ Distance Check │ Timing Condition │ Return │ ├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤ │ step_distance < remaining_distance│ No arrival time specified │ EntityStatus │ │ (waypoint not reached yet) │ (remaining_time = NaN) │ move toward waypoint │ ├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤ │ step_distance < remaining_distance│ Arrival time not reached │ EntityStatus │ │ (waypoint not reached yet) │ (remaining_time ≥ step/2) │ move toward waypoint │ ├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤ │ step_distance ≥ remaining_distance│ No arrival time specified │ Recurse │ │ (waypoint reached in this step) │ (remaining_time = NaN) │ (advance waypoint) │ ├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤ │ step_distance ≥ remaining_distance│ Arrival time reached │ Recurse │ │ (waypoint reached in this step) │ (remaining_time < step/2) │ (advance waypoint) │ ├───────────────────────────────────┼──────────────────────────────┼──────────────────────┤ │ step_distance < remaining_distance│ Arrival time reached │ throw │ │ (waypoint not reached yet) │ (remaining_time < step/2) │ Error │ └───────────────────────────────────┴──────────────────────────────┴──────────────────────┘

Note: debug; search starts from vertices[1], skipping previous_waypoint (vertices[0]); returns true when no more timed waypoints exist or the nearest timed waypoint is the final waypoint; returns distance between two positions along lanelet; falls back to Euclidean distance if lanelet matching fails

DIRTY HACK! Negative longitudinal distance (calculated along lanelet in opposite direction) causes some scenarios to fail because of an unrelated issue with lanelet matching. The issue is caused by wrongly matched lanelet poses and thus wrong distances. When lanelet matching errors are fixed, this dirty hack can be removed.

Note: returns distance from entity to specified waypoint; special case: distance from entity to previous waypoint (vertices[0]); distance from entity to target waypoint (vertices[1]); start from vertices[1] (target waypoint), accumulate distance to waypoint_iterator; returns distance from entity to target waypoint (vertices[1]); returns distance from entity to nearest timed waypoint or final waypoint; returns remaining time to specified waypoint; returns quiet_NaN if waypoint has no time constraint; returns epsilon instead of 0.0 when exactly at arrival time; returns remaining time to target waypoint (vertices[1]); returns remaining time to nearest timed waypoint or final waypoint; returns quiet_NaN if no timed waypoints exist (indicates no time constraint); check entity position for NaN/infinity; check linear acceleration for NaN/infinity; check angular acceleration for NaN/infinity; check linear velocity for NaN/infinity; check angular velocity for NaN/infinity; check trajectory has at least 2 waypoints (previous and target); check previous waypoint position for NaN/infinity; check target waypoint position for NaN/infinity; check if vehicle has not exceeded arrival time for timed waypoint; allow delay of 1 step time due to floating point accumulation; check max acceleration for NaN/infinity; check min acceleration (max deceleration) for NaN/infinity; check desired acceleration for NaN/infinity; check desired speed for NaN/infinity; check desired velocity for NaN/infinity; refine position when entity crosses lanelet boundary during a single simulation step; detects transition by comparing current and updated lanelet_id, then adjusts position for precision; returns refined position if transition occurs and refinement succeeds, otherwise nullopt; if entity crosses lanelet boundary during this step, refine position for improved precision; desired_velocity is in global frame, but twist.linear is in local (entity) frame; in local frame, vehicle moves only forward (x-axis), so y and z components are zero; use pitch from lanelet if entity is on lane, otherwise use pitch on target; always use yaw on target; this_step_distance does not affect final waypoint handling logic, so it's not displayed for final waypoints; handle intermediate waypoint without arrival time defined; handle intermediate waypoint with arrival time defined; use step_time/2 threshold to account for floating point accumulation errors; distance sufficient and vehicle immobile, but arrival time not yet reached - wait with zero velocity; only report error if entity progressed past previous waypoint but stopped too far from target; entities waiting near previous waypoint (e.g., target_speed=0.0 at trajectory start) are allowed to be immobile

Todo:: distance within threshold but vehicle still moving - apply constrained braking to reach immobile state

Note: debug; search starts from vertices[1], skipping previous_waypoint (vertices[0]); returns true when no more timed waypoints exist or the nearest timed waypoint is the final waypoint; returns distance between two positions along lanelet; falls back to Euclidean distance if lanelet matching fails

DIRTY HACK! Negative longitudinal distance (calculated along lanelet in opposite direction) causes some scenarios to fail because of an unrelated issue with lanelet matching. The issue is caused by wrongly matched lanelet poses and thus wrong distances. When lanelet matching errors are fixed, this dirty hack can be removed.

Note: returns distance from entity to specified waypoint; special case: distance from entity to previous waypoint (vertices[0]); distance from entity to target waypoint (vertices[1]); start from vertices[1] (target waypoint), accumulate distance to waypoint_iterator; returns distance from entity to target waypoint (vertices[1]); returns distance from entity to nearest timed waypoint or final waypoint; returns remaining time to specified waypoint; returns quiet_NaN if waypoint has no time constraint; returns epsilon instead of 0.0 when exactly at arrival time; returns remaining time to target waypoint (vertices[1]); returns remaining time to nearest timed waypoint or final waypoint; returns quiet_NaN if no timed waypoints exist (indicates no time constraint); check entity position for NaN/infinity; check linear acceleration for NaN/infinity; check angular acceleration for NaN/infinity; check linear velocity for NaN/infinity; check angular velocity for NaN/infinity; check trajectory has at least 2 waypoints (previous and target); check previous waypoint position for NaN/infinity; check target waypoint position for NaN/infinity; check if vehicle has not exceeded arrival time for timed waypoint; allow delay of 1 step time due to floating point accumulation; check max acceleration for NaN/infinity; check min acceleration (max deceleration) for NaN/infinity; check desired acceleration for NaN/infinity; check desired speed for NaN/infinity; check desired velocity for NaN/infinity; refine position when entity crosses lanelet boundary during a single simulation step; detects transition by comparing current and updated lanelet_id, then adjusts position for precision; returns refined position if transition occurs and refinement succeeds, otherwise nullopt; if entity crosses lanelet boundary during this step, refine position for improved precision; desired_velocity is in global frame, but twist.linear is in local (entity) frame; in local frame, vehicle moves only forward (x-axis), so y and z components are zero; use pitch from lanelet if entity is on lane, otherwise use pitch on target; always use yaw on target; this_step_distance does not affect final waypoint handling logic, so it's not displayed for final waypoints; handle intermediate waypoint without arrival time defined; handle intermediate waypoint with arrival time defined; use step_time/2 threshold to account for floating point accumulation errors; distance sufficient and vehicle immobile, but arrival time not yet reached - wait with zero velocity; only report error if entity progressed past previous waypoint but stopped too far from target; entities waiting near previous waypoint (e.g., target_speed=0.0 at trajectory start) are allowed to be immobile

Todo:: distance within threshold but vehicle still moving - apply constrained braking to reach immobile state

Classes

Functions

Function Documentation

◆ makeUpdatedStatus()