class that defines the path smoothing algorithm More...

#include <smoothing.hpp>

Collaboration diagram for PathSmoothing:

Public Member Functions
	PathSmoothing ()=default
	Construct a new default Path Smoothing object.

	PathSmoothing (PathSmoothingConfig config)
	Construct a new Path Smoothing object with a given configuration.

std::vector< PathPoint >	smooth_path (const std::vector< PathPoint > &path, bool is_path_closed) const
	Smooths a path by fitting a B-spline through the input points.

std::vector< PathPoint >	optimize_path (const std::vector< PathPoint > &path, const std::vector< PathPoint > &yellow_cones, const std::vector< PathPoint > &blue_cones) const
	Optimizes a racing line path by fitting splines through track boundaries and applying quadratic programming optimization.

Private Member Functions
std::vector< PathPoint >	filter_path (const std::vector< PathPoint > &path) const
	Filters path points using a minimum spacing constraint.

std::vector< PathPoint >	osqp_optimization (const std::vector< PathPoint > &center, const std::vector< PathPoint > &left, const std::vector< PathPoint > &right) const
	Optimizes a path using quadratic programming (OSQP) to balance smoothness, curvature, and safety constraints.

void	add_curvature_terms (int num_path_points, const std::function< int(int)> &circular_index, const std::function< void(int, int, double)> &add_coefficient) const
	Adds curvature penalty terms to the quadratic objective function.

void	add_slack_penalty_terms (int num_path_points, const std::function< void(int, int, double)> &add_coefficient) const
	Adds penalty terms for slack variables to the quadratic objective function.

void	add_slack_penalty_terms (std::map< std::pair< int, int >, double > &quadratic_terms, int num_path_points, const std::function< void(int, int, double)> &add_coefficient) const
	Adds penalty terms for slack variables to the quadratic objective function.

void	add_boundary_constraints (std::vector< OSQPFloat > &constraint_values, std::vector< OSQPInt > &constraint_row_indices, std::vector< OSQPInt > &constraint_col_indices, std::vector< OSQPFloat > &constraint_lower_bounds, std::vector< OSQPFloat > &constraint_upper_bounds, int &constraint_count, const std::vector< PathPoint > &left, const std::vector< PathPoint > &right, int num_path_points, double safety_margin) const
	Adds track boundary constraints to ensure the optimized path stays within the track.

void	add_slack_nonnegativity_constraints (std::vector< OSQPFloat > &constraint_values, std::vector< OSQPInt > &constraint_row_indices, std::vector< OSQPInt > &constraint_col_indices, std::vector< OSQPFloat > &constraint_lower_bounds, std::vector< OSQPFloat > &constraint_upper_bounds, int &constraint_count, int num_path_points) const
	Adds non-negativity constraints for slack variables.

void	convert_to_csc_format (const std::vector< OSQPFloat > &values, const std::vector< OSQPInt > &row_indices, const std::vector< OSQPInt > &col_indices, int total_variables, std::vector< OSQPFloat > &csc_x, std::vector< OSQPInt > &csc_i, std::vector< OSQPInt > &csc_p) const
	Converts sparse matrix data from coordinate format to Compressed Sparse Column (CSC) format.

Private Attributes
PathSmoothingConfig	config_
	configuration of the smoothing algorithm

Detailed Description

class that defines the path smoothing algorithm

Definition at line 22 of file smoothing.hpp.

Constructor & Destructor Documentation

◆ PathSmoothing() [1/2]

PathSmoothing::PathSmoothing ( )

default

Construct a new default Path Smoothing object.

◆ PathSmoothing() [2/2]

PathSmoothing::PathSmoothing ( PathSmoothingConfig config )

inlineexplicit

Construct a new Path Smoothing object with a given configuration.

Definition at line 34 of file smoothing.hpp.

Member Function Documentation

◆ add_boundary_constraints()

void PathSmoothing::add_boundary_constraints	(	std::vector< OSQPFloat > &	constraint_values,
		std::vector< OSQPInt > &	constraint_row_indices,
		std::vector< OSQPInt > &	constraint_col_indices,
		std::vector< OSQPFloat > &	constraint_lower_bounds,
		std::vector< OSQPFloat > &	constraint_upper_bounds,
		int &	constraint_count,
		const std::vector< PathPoint > &	left,
		const std::vector< PathPoint > &	right,
		int	num_path_points,
		double	safety_margin
	)		const

private

Adds track boundary constraints to ensure the optimized path stays within the track.

Parameters

constraint_values	Non-zero values in the constraint matrix
constraint_row_indices	Row indices for constraint matrix entries
constraint_col_indices	Column indices for constraint matrix entries
constraint_lower_bounds	Lower bounds for each constraint
constraint_upper_bounds	Upper bounds for each constraint
constraint_count	Running count of constraints added
left	Left track boundary points
right	Right track boundary points
num_path_points	Number of points in the path
safety_margin	Safety distance from track boundaries

Definition at line 89 of file smoothing.cpp.

Here is the caller graph for this function:

◆ add_curvature_terms()

void PathSmoothing::add_curvature_terms	(	int	num_path_points,
		const std::function< int(int)> &	circular_index,
		const std::function< void(int, int, double)> &	add_coefficient
	)		const

private

Adds curvature penalty terms to the quadratic objective function.

Parameters

num_path_points	Number of points in the path
circular_index	Lambda function for circular array indexing
add_coefficient	Lambda function to add coefficients to the objective matrix

Definition at line 42 of file smoothing.cpp.

Here is the caller graph for this function:

◆ add_slack_nonnegativity_constraints()

void PathSmoothing::add_slack_nonnegativity_constraints	(	std::vector< OSQPFloat > &	constraint_values,
		std::vector< OSQPInt > &	constraint_row_indices,
		std::vector< OSQPInt > &	constraint_col_indices,
		std::vector< OSQPFloat > &	constraint_lower_bounds,
		std::vector< OSQPFloat > &	constraint_upper_bounds,
		int &	constraint_count,
		int	num_path_points
	)		const

private

Adds non-negativity constraints for slack variables.

Parameters

constraint_values	Non-zero values in the constraint matrix
constraint_row_indices	Row indices for constraint matrix entries
constraint_col_indices	Column indices for constraint matrix entries
constraint_lower_bounds	Lower bounds for each constraint
constraint_upper_bounds	Upper bounds for each constraint
constraint_count	Running count of constraints added
num_path_points	Number of points in the path

Definition at line 142 of file smoothing.cpp.

Here is the caller graph for this function:

◆ add_slack_penalty_terms() [1/2]

void PathSmoothing::add_slack_penalty_terms	(	int	num_path_points,
		const std::function< void(int, int, double)> &	add_coefficient
	)		const

private

Adds penalty terms for slack variables to the quadratic objective function.

Parameters

num_path_points	Number of points in the path
add_coefficient	Lambda function to add coefficients to the objective matrix

Definition at line 78 of file smoothing.cpp.

Here is the caller graph for this function:

◆ add_slack_penalty_terms() [2/2]

void PathSmoothing::add_slack_penalty_terms	(	std::map< std::pair< int, int >, double > &	quadratic_terms,
		int	num_path_points,
		const std::function< void(int, int, double)> &	add_coefficient
	)		const

private

Adds penalty terms for slack variables to the quadratic objective function.

Parameters

quadratic_terms	Map storing quadratic coefficient terms
num_path_points	Number of points in the path
add_coefficient	Lambda function to add coefficients to the objective matrix

◆ convert_to_csc_format()

void PathSmoothing::convert_to_csc_format	(	const std::vector< OSQPFloat > &	values,
		const std::vector< OSQPInt > &	row_indices,
		const std::vector< OSQPInt > &	col_indices,
		int	total_variables,
		std::vector< OSQPFloat > &	csc_x,
		std::vector< OSQPInt > &	csc_i,
		std::vector< OSQPInt > &	csc_p
	)		const

private

Converts sparse matrix data from coordinate format to Compressed Sparse Column (CSC) format.

CSC format is required by the OSQP solver for efficient matrix operations.

Parameters

values	Non-zero values in coordinate format
row_indices	Row indices in coordinate format
col_indices	Column indices in coordinate format
total_variables	Number of columns in the matrix
csc_x	Output: non-zero values in CSC format
csc_i	Output: row indices in CSC format
csc_p	Output: column pointers in CSC format

Definition at line 161 of file smoothing.cpp.

Here is the caller graph for this function:

◆ filter_path()

std::vector< PathPoint > PathSmoothing::filter_path ( const std::vector< PathPoint > & path ) const

private

Filters path points using a minimum spacing constraint.

Parameters

path	Input path.

Returns: Filtered path.

Definition at line 31 of file smoothing.cpp.

Here is the caller graph for this function:

◆ optimize_path()

std::vector< PathPoint > PathSmoothing::optimize_path	(	const std::vector< PathPoint > &	path,
		const std::vector< PathPoint > &	yellow_cones,
		const std::vector< PathPoint > &	blue_cones
	)		const

Optimizes a racing line path by fitting splines through track boundaries and applying quadratic programming optimization.

Parameters

path	The initial center path to be optimized
yellow_cones	Track boundary markers on the right boundary
blue_cones	Track boundary markers on the left boundary

Returns: std::vector<PathPoint> The optimized path

Definition at line 14 of file smoothing.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ osqp_optimization()

std::vector< PathPoint > PathSmoothing::osqp_optimization	(	const std::vector< PathPoint > &	center,
		const std::vector< PathPoint > &	left,
		const std::vector< PathPoint > &	right
	)		const

private

Optimizes a path using quadratic programming (OSQP) to balance smoothness, curvature, and safety constraints.

The function takes a center line and left/right boundaries, then computes an optimized path that minimizes curvature and jerk while staying within the track boundaries with a safety margin.

Parameters

center	Sequence of points representing the initial center line path
left	Sequence of points representing the left track boundary
right	Sequence of points representing the right track boundary

Returns: std::vector<PathPoint> Optimized path

Definition at line 194 of file smoothing.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ smooth_path()

std::vector< PathPoint > PathSmoothing::smooth_path	(	const std::vector< PathPoint > &	path,
		bool	is_path_closed
	)		const

Smooths a path by fitting a B-spline through the input points.

Parameters

path	The input path to be smoothed
is_path_closed	Whether the path is closed

Returns: std::vector<PathPoint> The smoothed path, with closure point appended if applicable

Definition at line 3 of file smoothing.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ config_

PathSmoothingConfig PathSmoothing::config_

private

configuration of the smoothing algorithm

Definition at line 65 of file smoothing.hpp.

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

src/planning/include/planning/smoothing.hpp
src/planning/src/planning/smoothing.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PathSmoothing() [1/2]

◆ PathSmoothing() [2/2]

Member Function Documentation

◆ add_boundary_constraints()

◆ add_curvature_terms()

◆ add_slack_nonnegativity_constraints()

◆ add_slack_penalty_terms() [1/2]

◆ add_slack_penalty_terms() [2/2]

◆ convert_to_csc_format()

◆ filter_path()

◆ optimize_path()

◆ osqp_optimization()

◆ smooth_path()

Member Data Documentation

◆ config_