traffic_simulator::follow_trajectory::FollowWaypointController Class Reference

#include <follow_waypoint_controller.hpp>

Public Member Functions
constexpr	FollowWaypointController (const traffic_simulator_msgs::msg::BehaviorParameter &behavior_parameter, const double step_time, const bool with_braking, const std::optional< double > &target_speed=std::nullopt)
auto	accelerationWithJerkConstraint (const double current_speed, const double target_speed, const double acceleration_rate) const -> double
	Calculate the optimal acceleration for a single discrete step to reach a desired target speed while respecting a maximum jerk constraint. More...
auto	getFollowedWaypointDetails (const traffic_simulator_msgs::msg::PolylineTrajectory &polyline_trajectory) const -> std::string
auto	getPredictedWaypointArrivalState (const double step_acceleration, const double remaining_time, const double remaining_distance, const traffic_simulator_msgs::msg::EntityStatus &entity_status, const std::function< traffic_simulator_msgs::msg::EntityStatus(const traffic_simulator_msgs::msg::EntityStatus &, const geometry_msgs::msg::Vector3 &)> &update_entity_status, const std::function< double(const geometry_msgs::msg::Point &, const geometry_msgs::msg::Point &)> &distance_along_lanelet) const -> std::optional< PredictedEntityStatus >
auto	getAcceleration (const double remaining_distance, const traffic_simulator_msgs::msg::EntityStatus &entity_status, const std::function< traffic_simulator_msgs::msg::EntityStatus(const traffic_simulator_msgs::msg::EntityStatus &, const geometry_msgs::msg::Vector3 &)> &update_entity_status, const std::function< double(const geometry_msgs::msg::Point &, const geometry_msgs::msg::Point &)> &distance_along_lanelet) const -> double
auto	getAcceleration (const double remaining_time_source, const double remaining_distance, const traffic_simulator_msgs::msg::EntityStatus &entity_status, const std::function< traffic_simulator_msgs::msg::EntityStatus(const traffic_simulator_msgs::msg::EntityStatus &, const geometry_msgs::msg::Vector3 &)> &update_entity_status, const std::function< double(const geometry_msgs::msg::Point &, const geometry_msgs::msg::Point &)> &distance_along_lanelet) const -> double
auto	areConditionsOfArrivalMet (const double acceleration, const double speed, const double distance) const -> double

Static Public Attributes
static constexpr double	remaining_distance_tolerance = 0.1
static constexpr double	local_epsilon = 1e-12

Friends
template<typename StreamType >
auto	operator<< (StreamType &stream, const FollowWaypointController &c) -> StreamType &

Constructor & Destructor Documentation

FollowWaypointController()

constexpr traffic_simulator::follow_trajectory::FollowWaypointController::FollowWaypointController ( const traffic_simulator_msgs::msg::BehaviorParameter &  behavior_parameter, const double  step_time, const bool  with_braking, const std::optional< double > &  target_speed = std::nullopt  )

inlineexplicitconstexpr

Member Function Documentation

accelerationWithJerkConstraint()

auto traffic_simulator::follow_trajectory::FollowWaypointController::accelerationWithJerkConstraint	(	const double	current_speed,
		const double	target_speed,
		const double	acceleration_rate
	)		const -> double

Calculate the optimal acceleration for a single discrete step to reach a desired target speed while respecting a maximum jerk constraint.

This method computes the acceleration to be applied in the current time step so that the final speed after a series of discrete steps equals the target speed, assuming constant jerk (linear change of acceleration) over all steps.

Parameters

current_speed	The current speed at the start of the step [m/s].
target_speed	The desired target speed to reach after N discrete steps [m/s].
max_jerk	The maximum allowed rate of change of acceleration [m/s³].

Returns

The optimal acceleration to apply in the current step [m/s²].

Note

Algorithm derivation for discrete-time motion with constant jerk:

1. Estimating the number of steps N:

   For motion with constant jerk j, the velocity change over time t is:

   Δv = a₀·t + 0.5·j·t²

   Assuming we start from rest (a₀ = 0) for the jerk phase, the time required is:

   t = sqrt(2·|Δv| / j)

   Converting to discrete steps with step_time dt:

   N = round(t / dt) = round(sqrt(2·|Δv| / j) / dt)

   where Δv = target_speed - current_speed, and N ≥ 1.

2. Computing base acceleration:

   Without jerk, uniform acceleration to reach Δv over N steps would be:

   a_base = Δv / (N·dt)

3. Deriving the jerk correction:

   Why we need jerk correction: If we simply used a_base (constant acceleration), we would ignore the constraint that acceleration must change smoothly with limited jerk. By applying constant jerk, acceleration increases/decreases linearly over time, which means we accumulate additional velocity change beyond what a_base alone would provide. The jerk correction compensates for this by adjusting the initial acceleration a[0] so that the total velocity change after N steps exactly matches the desired Δv.

   With constant jerk j applied over N steps, acceleration changes linearly:

   a[k] = a[0] + j·dt·k, for k = 0, 1, ..., N-1

   Total velocity change is the sum of accelerations times dt:

   Δv = dt · Σ(a[k]) = dt · Σ(a[0] + j·dt·k) = dt · [N·a[0] + j·dt·Σ(k)] = dt · [N·a[0] + j·dt·(0 + 1 + ... + (N-1))] = dt · [N·a[0] + j·dt·(N·(N-1)/2)]

   Solving for a[0] (the initial acceleration to apply):

   a[0] = Δv/(N·dt) - j·dt·(N-1)/2

   Breaking this into components:

   a[0] = a_base + a_correction

   where:

   • a_base = Δv/(N·dt) (uniform acceleration component)
   • a_correction = -j·dt·(N-1)/2 (jerk correction)

   However, the sign of jerk depends on acceleration/deceleration direction:

   • For acceleration (Δv > 0): jerk is positive, correction is positive
   • For deceleration (Δv < 0): jerk is negative, correction is negative

   Therefore:

   a_correction = sign(Δv) · j · dt · (N-1) / 2

4. Final formula:

   a_optimal = a_base + a_correction = Δv/(N·dt) + sign(Δv)·j·dt·(N-1)/2

Note

• This is a discrete approximation; small errors may occur due to rounding of N.
• The method does not clip the acceleration to physical limits, so the caller must apply additional clamping to ensure the acceleration remains within max_acceleration and max_deceleration bounds.
• The formula is symmetric for acceleration and deceleration - only the sign of the jerk correction term changes based on the velocity difference direction.

Note

use absolute value of acceleration_rate to ensure it is always positive

minimal number of discrete steps required to reach target_speed based on maximum jerk and time step

acceleration (without jerk correction) required to reach target_speed assuming constant acceleration

sign of the jerk correction based on whether we are accelerating or decelerating

correction term due to linear change of acceleration (jerk), this ensures that after all discrete steps, the final speed matches target_speed

areConditionsOfArrivalMet()

auto traffic_simulator::follow_trajectory::FollowWaypointController::areConditionsOfArrivalMet ( const double  acceleration, const double  speed, const double  distance  ) const -> double

inline

getAcceleration() [1/2]

auto traffic_simulator::follow_trajectory::FollowWaypointController::getAcceleration	(	const double	remaining_distance,
		const traffic_simulator_msgs::msg::EntityStatus &	entity_status,
		const std::function< traffic_simulator_msgs::msg::EntityStatus(const traffic_simulator_msgs::msg::EntityStatus &, const geometry_msgs::msg::Vector3 &)> &	update_entity_status,
		const std::function< double(const geometry_msgs::msg::Point &, const geometry_msgs::msg::Point &)> &	distance_along_lanelet
	)		const -> double

Note

always prefer the smallest positive distance_diff (candidate stops before target) only accept negative distance_diff (overshooting) if no positive option exists

both stop before target - choose the one that stops closer to target

current stops before target, best overshoots - always prefer stopping before

both overshoot target - choose the one that overshoots less

else: current overshoots but best stops before target - keep best (should_update_best_candidate = false)

getAcceleration() [2/2]

auto traffic_simulator::follow_trajectory::FollowWaypointController::getAcceleration	(	const double	remaining_time_source,
		const double	remaining_distance,
		const traffic_simulator_msgs::msg::EntityStatus &	entity_status,
		const std::function< traffic_simulator_msgs::msg::EntityStatus(const traffic_simulator_msgs::msg::EntityStatus &, const geometry_msgs::msg::Vector3 &)> &	update_entity_status,
		const std::function< double(const geometry_msgs::msg::Point &, const geometry_msgs::msg::Point &)> &	distance_along_lanelet
	)		const -> double

Todo:

getFollowedWaypointDetails()

auto traffic_simulator::follow_trajectory::FollowWaypointController::getFollowedWaypointDetails ( const traffic_simulator_msgs::msg::PolylineTrajectory &  polyline_trajectory ) const -> std::string

inline

getPredictedWaypointArrivalState()

auto traffic_simulator::follow_trajectory::FollowWaypointController::getPredictedWaypointArrivalState	(	const double	step_acceleration,
		const double	remaining_time,
		const double	remaining_distance,
		const traffic_simulator_msgs::msg::EntityStatus &	entity_status,
		const std::function< traffic_simulator_msgs::msg::EntityStatus(const traffic_simulator_msgs::msg::EntityStatus &, const geometry_msgs::msg::Vector3 &)> &	update_entity_status,
		const std::function< double(const geometry_msgs::msg::Point &, const geometry_msgs::msg::Point &)> &	distance_along_lanelet
	)		const -> std::optional<PredictedEntityStatus>

Note

clampAcceleration handles edge cases (negative speed or speed > target_speed) by returning corrective acceleration.

Friends And Related Function Documentation

operator<<

template<typename StreamType >

auto operator<< ( StreamType &  stream, const FollowWaypointController &  c  ) -> StreamType &

friend

Member Data Documentation

local_epsilon

constexpr double traffic_simulator::follow_trajectory::FollowWaypointController::local_epsilon = 1e-12

staticconstexpr

remaining_distance_tolerance

constexpr double traffic_simulator::follow_trajectory::FollowWaypointController::remaining_distance_tolerance = 0.1

staticconstexpr

---

The documentation for this class was generated from the following files:

• simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp
• simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp