Responsible for path planning and trajectory generation for the autonomous vehicle. More...

#include <planning.hpp>

Inheritance diagram for Planning:

Collaboration diagram for Planning:

Public Member Functions
	Planning (const PlanningParameters &params)
	Constructs a Planning node with the specified configuration parameters.

void	set_mission (Mission mission)
	Sets the mission type for planning execution.

Static Public Member Functions
static PlanningParameters	load_config (std::string &adapter)
	Loads planning configuration from YAML files.

Private Member Functions
void	vehicle_localization_callback (const custom_interfaces::msg::Pose &message)
	Callback for vehicle localization pose updates.

void	track_map_callback (const custom_interfaces::msg::ConeArray &message)
	Callback for track map updates.

void	compute_path_orientation (std::vector< PathPoint > &path)
	Computes and assigns the orientation for each point in the path.

void	run_full_map ()
	Generates and optimizes a closed-loop global track path.

void	run_acceleration ()
	Executes planning for the acceleration mission.

void	run_autocross ()
	Executes planning for the autocross mission.

void	run_trackdrive ()
	Executes planning for the trackdrive mission.

void	fetch_discipline ()
	Fetches the current mission/discipline from the pacsim simulation service.

void	run_planning_algorithms ()
	Runs the appropriate planning algorithm based on current mission.

void	publish_path_points () const
	Publishes the final planned path to the control module.

void	publish_visualization_msgs () const
	Publishes all visualization markers.

void	publish_execution_time (rclcpp::Time start_time)
	Publishes the planning algorithm execution time.

virtual void	finish ()=0
	Called when planning mission is completed.

Private Attributes
Mission	mission_ = Mission::NONE

PlanningConfig	planning_config_

Pose	pose_

std::string	map_frame_id_

double	desired_velocity_

double	initial_car_orientation_

int	lap_counter_ = 0

PathCalculation	path_calculation_

PathSmoothing	path_smoothing_

VelocityPlanning	velocity_planning_

Skidpad	skidpad_

bool	is_braking_ = false

bool	has_received_track_ = false

bool	has_received_pose_ = false

bool	is_path_final_ = false

bool	is_path_closed_ = false

std::chrono::steady_clock::time_point	brake_time_

std::vector< PathPoint >	full_path_

std::vector< PathPoint >	smoothed_path_

std::vector< Cone >	cone_array_

rclcpp::Subscription< custom_interfaces::msg::Pose >::SharedPtr	vehicle_localization_sub_

rclcpp::Subscription< custom_interfaces::msg::ConeArray >::SharedPtr	track_map_sub_

rclcpp::Subscription< std_msgs::msg::Float64 >::SharedPtr	lap_counter_sub_

rclcpp::Publisher< custom_interfaces::msg::PathPointArray >::SharedPtr	path_pub_
	< Publisher of the smoothed path to control

rclcpp::Publisher< std_msgs::msg::Float64 >::SharedPtr	planning_execution_time_pub_

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	yellow_cones_pub_
	< Publisher for the yellow cones

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	blue_cones_pub_
	Publisher for Delaunay triangulations.

rclcpp::Publisher< visualization_msgs::msg::Marker >::SharedPtr	triangulations_pub_
	Publisher for the past portion of the path (from the start to a lookback distance behind the car’s current position)

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	path_to_car_pub_
	Publisher for the entire planned path (from start to finish)

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	full_path_pub_
	Publisher for the smoothed path.

rclcpp::Publisher< visualization_msgs::msg::Marker >::SharedPtr	smoothed_path_pub_
	Publisher for velocity hover markers.

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	velocity_hover_pub_

rclcpp::Client< rcl_interfaces::srv::GetParameters >::SharedPtr	param_client_

Friends
class	PacSimAdapter

class	EufsAdapter

class	FsdsAdapter

class	VehicleAdapter

Detailed Description

Responsible for path planning and trajectory generation for the autonomous vehicle.

The Planning class handles multiple mission types (acceleration, skidpad, autocross, trackdrive) and generates optimal paths based on cone positions. It subscribes to vehicle localization and track map data, performs path calculation and smoothing, applies velocity planning, and publishes the final trajectory for the control module.

This class integrates several planning modules:

Path calculation using computational geometry
Path smoothing using spline interpolation
Velocity planning with acceleration constraints
Skidpad-specific planning

Note: This class inherits from rclcpp::Node and operates as a ROS 2 node.

Definition at line 54 of file planning.hpp.

Constructor & Destructor Documentation

◆ Planning()

Planning::Planning ( const PlanningParameters & params )

explicit

Constructs a Planning node with the specified configuration parameters.

Initializes all planning modules, subscriptions, and publishers. Sets up communication with state estimation and pacsim services. Determines the operating adapter type and configures frame IDs accordingly.

Parameters

params Configuration parameters loaded from YAML files

Definition at line 102 of file planning.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ compute_path_orientation()

void Planning::compute_path_orientation ( std::vector< PathPoint > & path )

private

Computes and assigns the orientation for each point in the path.

Parameters

path	The path whose points will have their orientation field updated in-place.

Definition at line 233 of file planning.cpp.

Here is the caller graph for this function:

◆ fetch_discipline()

void Planning::fetch_discipline ( )

private

Fetches the current mission/discipline from the pacsim simulation service.

Definition at line 161 of file planning.cpp.

Here is the caller graph for this function:

◆ finish()

virtual void Planning::finish ( )

privatepure virtual

Called when planning mission is completed.

Implemented in EufsAdapter, FsdsAdapter, VehicleAdapter, EufsAdapter, FsdsAdapter, PacSimAdapter, VehicleAdapter, and VehicleAdapter.

◆ load_config()

PlanningParameters Planning::load_config ( std::string & adapter )

static

Loads planning configuration from YAML files.

Reads global configuration to determine the adapter type, then loads adapter-specific planning parameters from the corresponding YAML file.

Parameters

adapter Output parameter that stores the adapter type ("eufs", "pacsim", "vehicle")

Returns: PlanningParameters Struct containing all loaded configuration parameters

Definition at line 11 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publish_execution_time()

void Planning::publish_execution_time ( rclcpp::Time start_time )

private

Publishes the planning algorithm execution time.

Parameters

start_time ROS time when planning algorithms began execution

Definition at line 458 of file planning.cpp.

Here is the caller graph for this function:

◆ publish_path_points()

void Planning::publish_path_points ( ) const

private

Publishes the final planned path to the control module.

Definition at line 451 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publish_visualization_msgs()

void Planning::publish_visualization_msgs ( ) const

private

Publishes all visualization markers.

Publishes triangulation, full path, smoothed path, and path_to_car markers.

Definition at line 465 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_acceleration()

void Planning::run_acceleration ( )

private

Executes planning for the acceleration mission.

Definition at line 327 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_autocross()

void Planning::run_autocross ( )

private

Executes planning for the autocross mission.

On the first lap, calculates and smooths the path with velocity planning. On subsequent laps, reuses the calculated full path and applies stopping logic after the first lap completion.

Definition at line 334 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_full_map()

void Planning::run_full_map ( )

private

Generates and optimizes a closed-loop global track path.

Note: Intended to run once when the map is considered complete.

Definition at line 271 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_planning_algorithms()

void Planning::run_planning_algorithms ( )

private

Runs the appropriate planning algorithm based on current mission.

Dispatches to mission-specific planning functions and publishes the resulting trajectory.

Definition at line 390 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_trackdrive()

void Planning::run_trackdrive ( )

private

Executes planning for the trackdrive mission.

Implements multi-lap logic:

Lap 0: Explores track and builds initial path
Laps 1-9: Uses optimized full track path with trackdrive velocity planning
Lap 10+: Brings vehicle to stop using full path

Definition at line 361 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_mission()

void Planning::set_mission ( Mission mission )

Sets the mission type for planning execution.

Parameters

mission The mission type to execute

Definition at line 199 of file planning.cpp.

Here is the caller graph for this function:

◆ track_map_callback()

void Planning::track_map_callback ( const custom_interfaces::msg::ConeArray & message )

private

Callback for track map updates.

Processes incoming cone detections and triggers run_planning_algorithms() if vehicle pose has already been received.

Parameters

message The received ConeArray message

Definition at line 220 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ vehicle_localization_callback()

void Planning::vehicle_localization_callback ( const custom_interfaces::msg::Pose & message )

private

Callback for vehicle localization pose updates.

Updates the current vehicle pose and triggers run_planning_algorithms() if track map have been received.

Parameters

message The received Pose message containing vehicle position and orientation

Definition at line 203 of file planning.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

Friends And Related Symbol Documentation

◆ EufsAdapter

friend class EufsAdapter

friend

Definition at line 86 of file planning.hpp.

◆ FsdsAdapter

friend class FsdsAdapter

friend

Definition at line 87 of file planning.hpp.

◆ PacSimAdapter

friend class PacSimAdapter

friend

Definition at line 85 of file planning.hpp.

◆ VehicleAdapter

friend class VehicleAdapter

friend

Definition at line 88 of file planning.hpp.

Member Data Documentation

◆ blue_cones_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr Planning::blue_cones_pub_

private

Publisher for Delaunay triangulations.

Definition at line 134 of file planning.hpp.

◆ brake_time_

std::chrono::steady_clock::time_point Planning::brake_time_

private

Definition at line 112 of file planning.hpp.

◆ cone_array_

std::vector<Cone> Planning::cone_array_

private

Definition at line 118 of file planning.hpp.

◆ desired_velocity_

double Planning::desired_velocity_

private

Definition at line 96 of file planning.hpp.

◆ full_path_

std::vector<PathPoint> Planning::full_path_

private

Definition at line 116 of file planning.hpp.

◆ full_path_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr Planning::full_path_pub_

private

Publisher for the smoothed path.

Definition at line 141 of file planning.hpp.

◆ has_received_pose_

bool Planning::has_received_pose_ = false

private

Definition at line 109 of file planning.hpp.

◆ has_received_track_

bool Planning::has_received_track_ = false

private

Definition at line 108 of file planning.hpp.

◆ initial_car_orientation_

double Planning::initial_car_orientation_

private

Definition at line 97 of file planning.hpp.

◆ is_braking_

bool Planning::is_braking_ = false

private

Definition at line 107 of file planning.hpp.

◆ is_path_closed_

bool Planning::is_path_closed_ = false

private

Definition at line 111 of file planning.hpp.

◆ is_path_final_

bool Planning::is_path_final_ = false

private

Definition at line 110 of file planning.hpp.

◆ lap_counter_

int Planning::lap_counter_ = 0

private

Definition at line 98 of file planning.hpp.

◆ lap_counter_sub_

rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr Planning::lap_counter_sub_

private

Definition at line 123 of file planning.hpp.

◆ map_frame_id_

std::string Planning::map_frame_id_

private

Definition at line 95 of file planning.hpp.

◆ mission_

Mission Planning::mission_ = Mission::NONE

private

Definition at line 92 of file planning.hpp.

◆ param_client_

rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr Planning::param_client_

private

Definition at line 148 of file planning.hpp.

◆ path_calculation_

PathCalculation Planning::path_calculation_

private

Definition at line 101 of file planning.hpp.

◆ path_pub_

rclcpp::Publisher<custom_interfaces::msg::PathPointArray>::SharedPtr Planning::path_pub_

private

< Publisher of the smoothed path to control

Definition at line 127 of file planning.hpp.

◆ path_smoothing_

PathSmoothing Planning::path_smoothing_

private

Definition at line 102 of file planning.hpp.

◆ path_to_car_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr Planning::path_to_car_pub_

private

Publisher for the entire planned path (from start to finish)

Definition at line 139 of file planning.hpp.

◆ planning_config_

PlanningConfig Planning::planning_config_

private

Definition at line 93 of file planning.hpp.

◆ planning_execution_time_pub_

rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr Planning::planning_execution_time_pub_

private

Definition at line 128 of file planning.hpp.

◆ pose_

Pose Planning::pose_

private

Definition at line 94 of file planning.hpp.

◆ skidpad_

Skidpad Planning::skidpad_

private

Definition at line 104 of file planning.hpp.

◆ smoothed_path_

std::vector<PathPoint> Planning::smoothed_path_

private

Definition at line 117 of file planning.hpp.

◆ smoothed_path_pub_

rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr Planning::smoothed_path_pub_

private

Publisher for velocity hover markers.

Definition at line 143 of file planning.hpp.

◆ track_map_sub_

rclcpp::Subscription<custom_interfaces::msg::ConeArray>::SharedPtr Planning::track_map_sub_

private

Definition at line 122 of file planning.hpp.

◆ triangulations_pub_

rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr Planning::triangulations_pub_

private

Publisher for the past portion of the path (from the start to a lookback distance behind the car’s current position)

Definition at line 136 of file planning.hpp.

◆ vehicle_localization_sub_

rclcpp::Subscription<custom_interfaces::msg::Pose>::SharedPtr Planning::vehicle_localization_sub_

private

Definition at line 121 of file planning.hpp.

◆ velocity_hover_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr Planning::velocity_hover_pub_

private

Definition at line 145 of file planning.hpp.

◆ velocity_planning_

VelocityPlanning Planning::velocity_planning_

private

Definition at line 103 of file planning.hpp.

◆ yellow_cones_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr Planning::yellow_cones_pub_

private

< Publisher for the yellow cones

Publisher for the blue cones

Definition at line 132 of file planning.hpp.

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

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

Public Member Functions

Static Public Member Functions

Private Member Functions

Private Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ Planning()

Member Function Documentation

◆ compute_path_orientation()

◆ fetch_discipline()

◆ finish()

◆ load_config()

◆ publish_execution_time()

◆ publish_path_points()

◆ publish_visualization_msgs()

◆ run_acceleration()

◆ run_autocross()

◆ run_full_map()

◆ run_planning_algorithms()

◆ run_trackdrive()

◆ set_mission()

◆ track_map_callback()

◆ vehicle_localization_callback()

Friends And Related Symbol Documentation

◆ EufsAdapter

◆ FsdsAdapter

◆ PacSimAdapter

◆ VehicleAdapter

Member Data Documentation

◆ blue_cones_pub_

◆ brake_time_

◆ cone_array_

◆ desired_velocity_

◆ full_path_

◆ full_path_pub_

◆ has_received_pose_

◆ has_received_track_

◆ initial_car_orientation_

◆ is_braking_

◆ is_path_closed_

◆ is_path_final_

◆ lap_counter_

◆ lap_counter_sub_

◆ map_frame_id_

◆ mission_

◆ param_client_

◆ path_calculation_

◆ path_pub_

◆ path_smoothing_

◆ path_to_car_pub_

◆ planning_config_

◆ planning_execution_time_pub_

◆ pose_

◆ skidpad_

◆ smoothed_path_

◆ smoothed_path_pub_

◆ track_map_sub_

◆ triangulations_pub_

◆ vehicle_localization_sub_

◆ velocity_hover_pub_

◆ velocity_planning_

◆ yellow_cones_pub_