Computes velocity profiles for a planned path based on curvature and dynamics constraints. More...

#include <velocity_planning.hpp>

Collaboration diagram for VelocityPlanning:

Public Member Functions
	VelocityPlanning ()=default
	Construct a new default Velocity Planning object.

	VelocityPlanning (VelocityPlanningConfig config)
	Construct a new Velocity Planning object with a given configuration.

void	set_velocity (std::vector< PathPoint > &final_path)
	Assigns an velocity to each point of the path.

void	trackdrive_velocity (std::vector< PathPoint > &final_path)
	Computes velocity for trackdrive scenarios.

void	stop (std::vector< PathPoint > &final_path, double braking_distance)
	Applies a braking velocity profile starting after a given braking distance.

Private Member Functions
double	find_curvature (const PathPoint &p1, const PathPoint &p2, const PathPoint &p3)
	Computes the curvature at a point using the Menger curvature formula.

void	point_speed (const std::vector< double > &curvatures, std::vector< double > &velocities)
	Computes the maximum velocity at each point based on curvature constraints.

void	acceleration_limiter (const std::vector< PathPoint > &points, std::vector< double > &velocities, const std::vector< double > &curvatures)
	Limits velocities based on forward acceleration constraints and friction ellipse.

void	braking_limiter (std::vector< PathPoint > &points, std::vector< double > &velocities, const std::vector< double > &curvatures)
	Limits velocities based on backward braking constraints and friction ellipse.

Private Attributes
VelocityPlanningConfig	config_
	configuration of the velocity planning algorithm

Static Private Attributes
static constexpr double	epsilon = 1e-9
	Numerical tolerance for floating-point comparisons.

Detailed Description

Computes velocity profiles for a planned path based on curvature and dynamics constraints.

The VelocityPlanning class generates a velocity profile along a path by:

Estimating curvature using the Menger curvature formula (circle fitting through three points),
Deriving maximum allowable velocities from lateral acceleration limits,
Applying a friction ellipse model to account for combined longitudinal and lateral tire forces,
Propagating acceleration constraints forward and braking constraints backward along the path.

The velocity planner respects the friction circle constraint: a_x² + a_y² ≤ a_max², ensuring that the vehicle stays within tire grip limits during combined cornering and acceleration/braking.

Definition at line 23 of file velocity_planning.hpp.

Constructor & Destructor Documentation

◆ VelocityPlanning() [1/2]

VelocityPlanning::VelocityPlanning ( )

default

Construct a new default Velocity Planning object.

◆ VelocityPlanning() [2/2]

VelocityPlanning::VelocityPlanning ( VelocityPlanningConfig config )

inlineexplicit

Construct a new Velocity Planning object with a given configuration.

Definition at line 34 of file velocity_planning.hpp.

Member Function Documentation

◆ acceleration_limiter()

void VelocityPlanning::acceleration_limiter	(	const std::vector< PathPoint > &	points,
		std::vector< double > &	velocities,
		const std::vector< double > &	curvatures
	)

private

Limits velocities based on forward acceleration constraints and friction ellipse.

Parameters

points	Vector of path points (used for distance calculations)
velocities	Vector of velocities to be updated (input/output)
curvatures	Vector of curvature values (used to compute lateral acceleration)

Definition at line 41 of file velocity_planning.cpp.

Here is the caller graph for this function:

◆ braking_limiter()

void VelocityPlanning::braking_limiter	(	std::vector< PathPoint > &	points,
		std::vector< double > &	velocities,
		const std::vector< double > &	curvatures
	)

private

Limits velocities based on backward braking constraints and friction ellipse.

Parameters

points	Vector of path points (used for distance calculations)
velocities	Vector of velocities to be updated (input/output)
curvatures	Vector of curvature values (used to compute lateral acceleration)

Definition at line 66 of file velocity_planning.cpp.

Here is the caller graph for this function:

◆ find_curvature()

double VelocityPlanning::find_curvature	(	const PathPoint &	p1,
		const PathPoint &	p2,
		const PathPoint &	p3
	)

private

Computes the curvature at a point using the Menger curvature formula.

Parameters

p1	Previous point
p2	Current point where curvature is computed
p3	Next point

Returns: double Curvature value. Returns 0.0 for straight sections or nearly collinear points.

Definition at line 3 of file velocity_planning.cpp.

Here is the caller graph for this function:

◆ point_speed()

void VelocityPlanning::point_speed	(	const std::vector< double > &	curvatures,
		std::vector< double > &	velocities
	)

private

Computes the maximum velocity at each point based on curvature constraints.

Parameters

curvatures	Vector of curvature values for each path point
velocities	Output vector to store the computed maximum velocities

Definition at line 25 of file velocity_planning.cpp.

Here is the caller graph for this function:

◆ set_velocity()

void VelocityPlanning::set_velocity ( std::vector< PathPoint > & final_path )

Assigns an velocity to each point of the path.

Parameters

final_path Vector of path points to update with planned velocities.

Definition at line 97 of file velocity_planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stop()

void VelocityPlanning::stop	(	std::vector< PathPoint > &	final_path,
		double	braking_distance
	)

Applies a braking velocity profile starting after a given braking distance.

Parameters

final_path	Vector of path points to update with planned velocities.
braking_distance	Distance along the path before braking begins.

Definition at line 151 of file velocity_planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ trackdrive_velocity()

void VelocityPlanning::trackdrive_velocity ( std::vector< PathPoint > & final_path )

Computes velocity for trackdrive scenarios.

Parameters

final_path Vector of path points to update with planned velocities.

Definition at line 124 of file velocity_planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ config_

VelocityPlanningConfig VelocityPlanning::config_

private

configuration of the velocity planning algorithm

Definition at line 63 of file velocity_planning.hpp.

◆ epsilon

constexpr double VelocityPlanning::epsilon = 1e-9

staticconstexprprivate

Numerical tolerance for floating-point comparisons.

Definition at line 68 of file velocity_planning.hpp.

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

src/planning/include/planning/velocity_planning.hpp
src/planning/src/planning/velocity_planning.cpp

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ VelocityPlanning() [1/2]

◆ VelocityPlanning() [2/2]

Member Function Documentation

◆ acceleration_limiter()

◆ braking_limiter()

◆ find_curvature()

◆ point_speed()

◆ set_velocity()

◆ stop()

◆ trackdrive_velocity()

Member Data Documentation

◆ config_

◆ epsilon