Generates optimal local paths. More...

#include <path_calculation.hpp>

Collaboration diagram for PathCalculation:

Public Member Functions
	PathCalculation ()=default
	Construct a new default PathCalculation object.

	PathCalculation (const PathCalculationConfig &config)
	Construct a new PathCalculation object with configuration.

std::vector< PathPoint >	calculate_path (const std::vector< Cone > &cone_array)
	Generate a path from an array of cones.

std::vector< PathPoint >	calculate_trackdrive (const std::vector< Cone > &cone_array)
	Generate a closed loop path for trackdrive competition.

void	set_vehicle_pose (const common_lib::structures::Pose &vehicle_pose)
	Set the current vehicle position and orientation.

bool	is_map_closed (std::vector< PathPoint > &path) const
	Checks if the path forms a closed loop and closes it if the transition cost is below config_.close_cost_.

std::vector< PathPoint >	get_path_to_car () const
	Get the path from the start to a lookback distance behind the car’s current position.

const std::vector< std::pair< Point, Point > > &	get_triangulations () const
	Get the triangulation segments used in path generation.

const std::vector< PathPoint > &	get_yellow_cones () const
	Gets the list of yellow cones detected or used in the path calculation.

const std::vector< PathPoint > &	get_blue_cones () const
	Gets the list of blue cones detected or used in the path calculation.

Private Member Functions
void	initialize_path_from_initial_pose ()
	Initialize the path from the vehicle's initial pose.

void	update_path_from_past_path ()
	Initialize the path from the previous path.

void	extend_path (int max_points)
	Extend the path by adding midpoints.

void	clear_path_state ()
	Reset path construction state.

int	reset_path (bool should_reset)
	Handle path reset when completing a lap.

std::pair< std::shared_ptr< Midpoint >, std::shared_ptr< Midpoint > >	select_starting_midpoints ()
	Select the first two midpoints to start the path.

std::vector< std::shared_ptr< Midpoint > >	select_candidate_midpoints (const Midpoint &anchor_pose, int num_candidates) const
	Select candidate midpoints in front of the vehicle.

std::pair< double, std::shared_ptr< Midpoint > >	find_best_next_midpoint (int depth, const std::shared_ptr< Midpoint > &previous, const std::shared_ptr< Midpoint > &current, double max_cost) const
	Recursively find the best next midpoint using depth-first search.

double	calculate_cost (double previous_x, double previous_y, double current_x, double current_y, double next_x, double next_y) const
	Calculate the cost of transitioning through three consecutive points.

void	discard_cones_along_path ()
	Invalidate cones and midpoints along the last path segment.

void	discard_cone (const std::shared_ptr< Midpoint > &last_mp, const std::shared_ptr< Midpoint > &current_mp)
	Determine which cone to discard between two consecutive midpoints.

void	invalidate_midpoints_with_discarded_cones ()
	Mark midpoints as invalid if they use discarded cones.

void	remove_invalid_neighbors ()
	Remove invalid neighbors from all midpoint connection lists.

std::shared_ptr< Midpoint >	find_nearest_midpoint (const Point &target) const
	Find the nearest valid midpoint to a target position.

std::pair< int, double >	find_best_loop_closure (const std::vector< PathPoint > &path) const
	Finds the best loop closure point in a path.

std::vector< PathPoint >	get_path_points_from_colorpoints (const std::vector< Colorpoint > &colorpoints) const
	Converts a vector of Colorpoint objects into a vector of PathPoint objects.

Private Attributes
PathCalculationConfig	config_

MidpointGenerator	midpoint_generator_

common_lib::structures::Pose	initial_pose_

common_lib::structures::Pose	vehicle_pose_

bool	initial_pose_set_ = false

std::vector< Colorpoint >	path_to_car_

std::vector< Colorpoint >	past_path_

int	reset_path_counter_ = 0

std::vector< std::shared_ptr< Midpoint > >	midpoints_

std::vector< Colorpoint >	current_path_

std::vector< PathPoint >	yellow_cones_

std::vector< PathPoint >	blue_cones_

std::unordered_map< Point, std::shared_ptr< Midpoint > >	point_to_midpoint_

std::unordered_set< std::shared_ptr< Midpoint > >	visited_midpoints_

std::unordered_set< std::shared_ptr< Cone > >	discarded_cones_

Detailed Description

Generates optimal local paths.

PathCalculation computes navigation paths by creating midpoints between track cones and searching for optimal sequences using depth-first search with cost-based evaluation. It maintains continuity with previous paths and discards cones as they are passed.

Definition at line 33 of file path_calculation.hpp.

Constructor & Destructor Documentation

◆ PathCalculation() [1/2]

PathCalculation::PathCalculation ( )

default

Construct a new default PathCalculation object.

◆ PathCalculation() [2/2]

PathCalculation::PathCalculation ( const PathCalculationConfig & config )

inlineexplicit

Construct a new PathCalculation object with configuration.

Parameters

config Configuration parameters for path calculation behavior.

Definition at line 44 of file path_calculation.hpp.

Member Function Documentation

◆ calculate_cost()

double PathCalculation::calculate_cost	(	double	previous_x,
		double	previous_y,
		double	current_x,
		double	current_y,
		double	next_x,
		double	next_y
	)		const

private

Calculate the cost of transitioning through three consecutive points.

Computes a weighted cost based on the distance (current→next) and the angle change (previous→current→next). Lower costs indicate smoother, more efficient paths.

Parameters

previous_x,previous_y	Coordinates of the previous point
current_x,current_y	Coordinates of the current point
next_x,next_y	Coordinates of the next point

Returns: Total cost combining angular deviation and distance penalties

Definition at line 427 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ calculate_path()

std::vector< PathPoint > PathCalculation::calculate_path ( const std::vector< Cone > & cone_array )

Generate a path from an array of cones.

Processes cone positions to create an optimal navigation path. Updates the internal state to maintain path continuity between invocations.

Parameters

cone_array Array of cones representing the track.

Returns: Vector of path points representing the calculated path.

Definition at line 13 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculate_trackdrive()

std::vector< PathPoint > PathCalculation::calculate_trackdrive ( const std::vector< Cone > & cone_array )

Generate a closed loop path for trackdrive competition.

Creates a path that forms a complete loop around the track, with interpolated connections and overlap points for continuous traversal.

Parameters

cone_array Array of cones representing the track.

Returns: Vector of path points forming a closed loop.

Definition at line 101 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ clear_path_state()

void PathCalculation::clear_path_state ( )

private

Reset path construction state.

Clears all internal tracking structures (current path, visited midpoints, discarded cones) to prepare for a fresh path calculation.

Definition at line 135 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ discard_cone()

void PathCalculation::discard_cone	(	const std::shared_ptr< Midpoint > &	last_mp,
		const std::shared_ptr< Midpoint > &	current_mp
	)

private

Determine which cone to discard between two consecutive midpoints.

Identifies the cone from the previous midpoint that is not used by the current midpoint, indicating it has been passed.

Parameters

last_mp	The previous midpoint in the path.
current_mp	The current midpoint in the path.

Definition at line 476 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ discard_cones_along_path()

void PathCalculation::discard_cones_along_path ( )

private

Invalidate cones and midpoints along the last path segment.

Marks cones as discarded if they were on the side of the path that has been passed. Invalidates any midpoints that depend on discarded cones and removes invalid neighbors from connectivity lists.

Definition at line 450 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ extend_path()

void PathCalculation::extend_path ( int max_points )

private

Extend the path by adding midpoints.

Iteratively searches for optimal next midpoints using depth-first search until reaching the maximum point limit or exhausting valid options.

Parameters

max_points Maximum number of new points to add.

Definition at line 243 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ find_best_loop_closure()

std::pair< int, double > PathCalculation::find_best_loop_closure ( const std::vector< PathPoint > & path ) const

private

Finds the best loop closure point in a path.

Parameters

path	A vector of PathPoint objects representing the path to analyze. Each PathPoint must have a valid position with x and y coordinates.

Returns

std::pair<int, double>

first: The index of the path point that represents the optimal loop closure.
second: The minimal combined cost associated with closing the loop at the returned index.

Definition at line 534 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ find_best_next_midpoint()

std::pair< double, std::shared_ptr< Midpoint > > PathCalculation::find_best_next_midpoint	(	int	depth,
		const std::shared_ptr< Midpoint > &	previous,
		const std::shared_ptr< Midpoint > &	current,
		double	max_cost
	)		const

private

Recursively find the best next midpoint using depth-first search.

Performs a bounded DFS to evaluate continuation paths from the current midpoint, balancing between direction consistency and distance.

Parameters

depth	Current search depth (decrements toward base case).
previous	Previous midpoint in the path.
current	Current midpoint being evaluated.
max_cost	Cost threshold for pruning branches.

Returns: Pair of accumulated cost and best next midpoint pointer.

Definition at line 388 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ find_nearest_midpoint()

std::shared_ptr< Midpoint > PathCalculation::find_nearest_midpoint ( const Point & target ) const

private

Find the nearest valid midpoint to a target position.

Performs a linear search through all midpoints to locate the closest one within the tolerance distance.

Parameters

target The target point to search around.

Returns: Pointer to the nearest midpoint, or nullptr if none within tolerance.

Definition at line 516 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ get_blue_cones()

const std::vector< PathPoint > & PathCalculation::get_blue_cones ( ) const

Gets the list of blue cones detected or used in the path calculation.

Returns: Constant reference to a vector containing the blue cones.

Definition at line 590 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ get_path_points_from_colorpoints()

std::vector< PathPoint > PathCalculation::get_path_points_from_colorpoints ( const std::vector< Colorpoint > & colorpoints ) const

private

Converts a vector of Colorpoint objects into a vector of PathPoint objects.

Parameters

points A vector containing the input Colorpoint objects.

Returns: A vector of PathPoint objects constructed from the given points.

Definition at line 567 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ get_path_to_car()

std::vector< PathPoint > PathCalculation::get_path_to_car ( ) const

Get the path from the start to a lookback distance behind the car’s current position.

Returns: Vector of path points representing the global path.

Definition at line 580 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_triangulations()

const std::vector< std::pair< Point, Point > > & PathCalculation::get_triangulations ( ) const

Get the triangulation segments used in path generation.

Returns: Reference to vector of point pairs representing triangulation edges.

Definition at line 584 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_yellow_cones()

const std::vector< PathPoint > & PathCalculation::get_yellow_cones ( ) const

Gets the list of yellow cones detected or used in the path calculation.

Returns: Constant reference to a vector containing the yellow cones.

Definition at line 588 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ initialize_path_from_initial_pose()

void PathCalculation::initialize_path_from_initial_pose ( )

private

Initialize the path from the vehicle's initial pose.

Selects the first two midpoints ahead of the vehicle to start the path when no previous path exists.

Definition at line 167 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ invalidate_midpoints_with_discarded_cones()

void PathCalculation::invalidate_midpoints_with_discarded_cones ( )

private

Mark midpoints as invalid if they use discarded cones.

Iterates through all midpoints and invalidates those whose defining cones have been marked as discarded.

Definition at line 491 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ is_map_closed()

bool PathCalculation::is_map_closed ( std::vector< PathPoint > & path ) const

Checks if the path forms a closed loop and closes it if the transition cost is below config_.close_cost_.

Parameters

path	The path to evaluate, modified in place if loop closure is found

Returns: true if loop closure was detected and applied, false Otherwise

Definition at line 80 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ remove_invalid_neighbors()

void PathCalculation::remove_invalid_neighbors ( )

private

Remove invalid neighbors from all midpoint connection lists.

Filters out neighbors that have been invalidated from each midpoint's close_points list to maintain consistency.

Definition at line 502 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ reset_path()

int PathCalculation::reset_path ( bool should_reset )

private

Handle path reset when completing a lap.

Adjusts the maximum points allowed and clears historical path data when completing a circuit.

Parameters

should_reset Whether to trigger a full path reset.

Returns: Adjusted maximum points for path extension.

Definition at line 145 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ select_candidate_midpoints()

std::vector< std::shared_ptr< Midpoint > > PathCalculation::select_candidate_midpoints	(	const Midpoint &	anchor_pose,
		int	num_candidates
	)		const

private

Select candidate midpoints in front of the vehicle.

Uses a priority queue to identify the most promising midpoints ahead of the vehicle based on combined distance and angle cost.

Parameters

anchor_pose	Reference pose to evaluate candidates from.
num_candidates	Number of candidates to return.

Returns: Vector of candidate midpoint pointers.

Definition at line 338 of file path_calculation.cpp.

Here is the caller graph for this function:

◆ select_starting_midpoints()

std::pair< std::shared_ptr< Midpoint >, std::shared_ptr< Midpoint > > PathCalculation::select_starting_midpoints ( )

private

Select the first two midpoints to start the path.

Identifies candidate midpoints ahead of the vehicle and performs a cost-based search to select the best starting pair.

Returns: Pair of midpoint pointers for path initialization.

Definition at line 295 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_vehicle_pose()

void PathCalculation::set_vehicle_pose ( const common_lib::structures::Pose & vehicle_pose )

Set the current vehicle position and orientation.

Propagates the vehicle pose to the midpoint generator and maintains the current pose for path calculations.

Parameters

vehicle_pose The current vehicle position and orientation.

Definition at line 160 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_path_from_past_path()

void PathCalculation::update_path_from_past_path ( )

private

Initialize the path from the previous path.

Snaps past path points to valid midpoints and carries forward as much of the previous path as possible, respecting distance constraints.

Definition at line 189 of file path_calculation.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ blue_cones_

std::vector<PathPoint> PathCalculation::blue_cones_

private

Definition at line 139 of file path_calculation.hpp.

◆ config_

PathCalculationConfig PathCalculation::config_

private

Definition at line 120 of file path_calculation.hpp.

◆ current_path_

std::vector<Colorpoint> PathCalculation::current_path_

private

Definition at line 137 of file path_calculation.hpp.

◆ discarded_cones_

std::unordered_set<std::shared_ptr<Cone> > PathCalculation::discarded_cones_

private

Definition at line 142 of file path_calculation.hpp.

◆ initial_pose_

common_lib::structures::Pose PathCalculation::initial_pose_

private

Definition at line 124 of file path_calculation.hpp.

◆ initial_pose_set_

bool PathCalculation::initial_pose_set_ = false

private

Definition at line 126 of file path_calculation.hpp.

◆ midpoint_generator_

MidpointGenerator PathCalculation::midpoint_generator_

private

Definition at line 121 of file path_calculation.hpp.

◆ midpoints_

std::vector<std::shared_ptr<Midpoint> > PathCalculation::midpoints_

private

Definition at line 134 of file path_calculation.hpp.

◆ past_path_

std::vector<Colorpoint> PathCalculation::past_path_

private

Definition at line 130 of file path_calculation.hpp.

◆ path_to_car_

std::vector<Colorpoint> PathCalculation::path_to_car_

private

Definition at line 129 of file path_calculation.hpp.

◆ point_to_midpoint_

std::unordered_map<Point, std::shared_ptr<Midpoint> > PathCalculation::point_to_midpoint_

private

Definition at line 140 of file path_calculation.hpp.

◆ reset_path_counter_

int PathCalculation::reset_path_counter_ = 0

private

Definition at line 131 of file path_calculation.hpp.

◆ vehicle_pose_

common_lib::structures::Pose PathCalculation::vehicle_pose_

private

Definition at line 125 of file path_calculation.hpp.

◆ visited_midpoints_

std::unordered_set<std::shared_ptr<Midpoint> > PathCalculation::visited_midpoints_

private

Definition at line 141 of file path_calculation.hpp.

◆ yellow_cones_

std::vector<PathPoint> PathCalculation::yellow_cones_

private

Definition at line 138 of file path_calculation.hpp.

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

src/planning/include/planning/path_calculation.hpp
src/planning/src/planning/path_calculation.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PathCalculation() [1/2]

◆ PathCalculation() [2/2]

Member Function Documentation

◆ calculate_cost()

◆ calculate_path()

◆ calculate_trackdrive()

◆ clear_path_state()

◆ discard_cone()

◆ discard_cones_along_path()

◆ extend_path()

◆ find_best_loop_closure()

◆ find_best_next_midpoint()

◆ find_nearest_midpoint()

◆ get_blue_cones()

◆ get_path_points_from_colorpoints()

◆ get_path_to_car()

◆ get_triangulations()

◆ get_yellow_cones()

◆ initialize_path_from_initial_pose()

◆ invalidate_midpoints_with_discarded_cones()

◆ is_map_closed()

◆ remove_invalid_neighbors()

◆ reset_path()

◆ select_candidate_midpoints()

◆ select_starting_midpoints()

◆ set_vehicle_pose()

◆ update_path_from_past_path()

Member Data Documentation

◆ blue_cones_

◆ config_

◆ current_path_

◆ discarded_cones_

◆ initial_pose_

◆ initial_pose_set_

◆ midpoint_generator_

◆ midpoints_

◆ past_path_

◆ path_to_car_

◆ point_to_midpoint_

◆ reset_path_counter_

◆ vehicle_pose_

◆ visited_midpoints_

◆ yellow_cones_