Classes and functionality for planning parking maneuvers. More...

Classes
class	AstarPathPlanner

class	BicycleModel
	Class implementing a kinematic bicycle model. More...

class	BicycleModelParameters
	Class for storing physical parameters of a bicycle model bicycle model. More...

class	Halfplane2D
	Class describing a halfplane in two-dimensional space. More...

class	NLPCostWeights

class	NLPObstacle
	Class used to represent an obstacle with the relevant data and conversions for the NLP solver. The idea is that this can be instantiated by the solver using MX as a template parameter to build constraints, then again using a concrete scalar type to call the solver with. More...

class	NLPObstacleStageVariables
	Class used to describe the additional dual variables required for each. More...

class	NLPPathPlanner

struct	NLPResults

class	ParkingPlanner
	Parking motion planner. More...

class	PlanningResult
	Results of a parking planning call. More...

class	Point2D
	Class describing a point in two-dimensional space. More...

class	Polytope2D
	Class describing a polytope in two-dimensional space. More...

struct	SerializedVariables

class	TrajectoryStep
	Class to represent one timestep in a dynamic trajectory. More...

class	VehicleCommand
	Class to represent the inputs of a vehicle. More...

class	VehicleState
	Class to represent the state of a vehicle. More...

Typedefs
template<typename T >
using	VehicleStateDerivative = parking_planner::VehicleState< T >

template<typename T >
using	Trajectory = std::vector< TrajectoryStep< T > >
	Class to represent a dynamic trajectory. More...

using	Point = geometry_msgs::msg::Point32

Enumerations
enum	PlanningStatus { PlanningStatus::OK, PlanningStatus::NLP_ERROR }
	Parking planner return codes. More...

Functions
template<typename T >
SerializedVariables< T >	assemble_variable_vector_and_bounds (const Trajectory< T > &trajectory, const std::vector< NLPObstacle< T >> &obstacles, const VehicleState< float64_t > &lower_state_bounds, const VehicleState< float64_t > &upper_state_bounds, const VehicleCommand< float64_t > &lower_command_bounds, const VehicleCommand< float64_t > &upper_command_bounds)
	Function to assemble the variable vector for the NLP from given states and commands. This is used in both the solver generation for specifying how the variables are ordered as well as when calling the solver because it will expect the initial guess in the same format. The adapter here serves as a single source of truth for how the serialization is to be done. More...

template<typename T >
Trajectory< float64_t >	disassemble_variable_vector (const std::vector< T > &variable_vector, const std::size_t horizon_length)
	Helper function to turn a serialized variable vector into a structured Trajectory object. More...

template<typename T >
std::vector< T >	assemble_parameter_vector (const VehicleState< T > &current_state, const VehicleState< T > &goal_state, const BicycleModelParameters< T > &model_parameters, const std::vector< NLPObstacle< T >> &obstacles, const NLPCostWeights< T > &cost_weights)
	Helper function to turn the inputs to an NLP solve into a flat vector of scalars for use by the CasADi-interfaced solver. More...

PARKING_PLANNER_PUBLIC std::vector< Polytope2D< float64_t > >	convert_drivable_area_to_obstacles (const lanelet::Polygon3d &drivable_area)

PARKING_PLANNER_PUBLIC autoware_auto_msgs::msg::Trajectory	convert_parking_planner_to_autoware_trajectory (const Trajectory< float64_t > &parking_trajectory)

template<typename T >
std::vector< T >	operator+ (const std::vector< T > &vector1, const std::vector< T > &vector2)

template<typename T >
std::vector< T >	operator* (const T &scalar, const std::vector< T > &vector)

template<class X , class U , typename P >
X	RK4 (X x, U u, P p, std::function< X(X, U, P)> f, const float64_t stepsize, const std::size_t steps)
	Template for an explicit Runge-Kutta integrator. CasADi has its own, but it for some reason doesn't compose with the IPOPT solver generator :shrug: More...

template<class X , class U >
X	RK4 (X x, U u, std::function< X(X, U)> f, const float64_t stepsize, const std::size_t steps)

static std::vector< VehicleState< float64_t > >	expand_state_longitudinal_with_heading (const VehicleState< float64_t > &from_state)

static bool	check_collision_bounding_box_vs_obstacles (const VehicleState< float64_t > &vehicle_state, const Polytope2D< float64_t > &bounding_box, const std::vector< Polytope2D< float64_t >> &obstacles)

static uint64_t	map_state_on_discretized_grid (const VehicleState< float64_t > &state, const VehicleState< float64_t > &reference)

static std::vector< NLPObstacleStageVariables< double > >	make_nlpobstacle_stage_variables (const double lambda_guess, const double mu_guess)

static NLPObstacle< double >	create_nlpobstacle (const Polytope2D< double > polytope, const double lambda_guess, const double mu_guess)

static NLPObstacle< double >	create_dummy_nlpobstacle (const double lambda_guess, const double mu_guess, const VehicleState< double > &current_state)

ConstraintBounds	create_constraint_bounds ()

std::vector< NLPObstacle< double > >	create_obstacles_from_polyhedra (const std::vector< Polytope2D< double >> &obstacles, const VehicleState< double > &current_state)

template<typename T >
static bool	are_vectors_close (std::vector< T > vector1, std::vector< T > vector2, double tolerance)

static std::vector< VehicleState< float64_t > >	resize_state_vector (const std::vector< VehicleState< float64_t >> &states_input, const std::size_t target_length)

static Point	lanelet_point_to_point (const lanelet::ConstPoint3d &lanelet_point)

bool	are_points_equal (const Point &p1, const Point &p2)

template<typename Iter1 , typename Iter2 >
static std::vector< std::list< Point > >	get_pocket_hulls (const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, const Iter2 convex_hull_end)

template<typename Iter >
static std::vector< std::list< Point > >	get_outer_boxes (const lanelet::Polygon3d &drivable_area, const Iter convex_hull_start, const Iter convex_hull_end)

Variables
static constexpr float64_t	MY_PI = 3.141592653589793

static constexpr float64_t	DELTA_LONGITUDINAL = 0.125

static const float64_t	DELTA_HEADING = MY_PI / 24.0

static constexpr float64_t	MAX_EXPLORATION_RADIUS = 30.0

static constexpr size_t	MAX_NUM_EXPLORATION_NODES = 1000000

constexpr std::size_t	HORIZON_LENGTH = 30
	NLP horizon length, roughly proportional to the maximum useful trajectory length. More...

constexpr std::size_t	MAX_NUMBER_OF_OBSTACLES = 10
	Maximum number of obstacles supported by the NLP planner. More...

constexpr std::size_t	MAX_HYPERPLANES_PER_OBSTACLE = 5
	Maximum number of hyperplanes per obstacle supported by the NLP planner. More...

constexpr std::size_t	MAX_EGO_HYPERPLANES = 4
	Maximum number of ego vehicle hyperplanes supported by the NLP planner. More...

constexpr float64_t	INTEGRATION_STEP_SIZE = 0.3
	Step size (in seconds) for the RK4 integration. More...

constexpr std::size_t	NUMBER_OF_INTEGRATION_STEPS = 4
	Number of (sub-)steps performed INTEGRATION_STEP_SIZE for the RK4 integration. More...

constexpr auto	ARTIFICIAL_DUAL_MULTIPLIER_BOUND = 1.0e4
	Since we need upper and lower bounds in the constructs used here, but some variables do not have clearly defined upper bounds, we introduce an artificial upper bound number that is "large enough to never matter". The value has been determined experimentally to work - picking this value too small will lead to the problem no longer being mathematically correct, making it too large will lead to numerical difficulties. More...

constexpr auto	LARGE_NEGATIVE_NUMBER = -1.0e4

constexpr auto	DUMMY_OBSTACLE_DISTANCE = 2e2

Detailed Description

Classes and functionality for planning parking maneuvers.

Typedef Documentation

◆ Point

using autoware::motion::planning::parking_planner::Point = typedef geometry_msgs::msg::Point32

◆ Trajectory

template<typename T >

using autoware::motion::planning::parking_planner::Trajectory = typedef std::vector<TrajectoryStep<T> >

Class to represent a dynamic trajectory.

◆ VehicleStateDerivative

template<typename T >

using autoware::motion::planning::parking_planner::VehicleStateDerivative = typedef parking_planner::VehicleState<T>

Enumeration Type Documentation

◆ PlanningStatus

enum autoware::motion::planning::parking_planner::PlanningStatus

strong

Parking planner return codes.

Enumerator
OK	Everything went well.
NLP_ERROR	There was an error running the nonlinear solver.

Function Documentation

◆ are_points_equal()

bool autoware::motion::planning::parking_planner::are_points_equal	(	const Point &	p1,
		const Point &	p2
	)

◆ are_vectors_close()

template<typename T >

static bool autoware::motion::planning::parking_planner::are_vectors_close	(	std::vector< T >	vector1,
		std::vector< T >	vector2,
		double	tolerance
	)

static

◆ assemble_parameter_vector()

template<typename T >

std::vector<T> autoware::motion::planning::parking_planner::assemble_parameter_vector	(	const VehicleState< T > &	current_state,
		const VehicleState< T > &	goal_state,
		const BicycleModelParameters< T > &	model_parameters,
		const std::vector< NLPObstacle< T >> &	obstacles,
		const NLPCostWeights< T > &	cost_weights
	)

Helper function to turn the inputs to an NLP solve into a flat vector of scalars for use by the CasADi-interfaced solver.

◆ assemble_variable_vector_and_bounds()

template<typename T >

SerializedVariables<T> autoware::motion::planning::parking_planner::assemble_variable_vector_and_bounds	(	const Trajectory< T > &	trajectory,
		const std::vector< NLPObstacle< T >> &	obstacles,
		const VehicleState< float64_t > &	lower_state_bounds,
		const VehicleState< float64_t > &	upper_state_bounds,
		const VehicleCommand< float64_t > &	lower_command_bounds,
		const VehicleCommand< float64_t > &	upper_command_bounds
	)

Function to assemble the variable vector for the NLP from given states and commands. This is used in both the solver generation for specifying how the variables are ordered as well as when calling the solver because it will expect the initial guess in the same format. The adapter here serves as a single source of truth for how the serialization is to be done.

The parameters for the bounds are only used when the solver is actually called, hence they are not templated - for assembly of the solver vectors, they are ignored and can be safely fed in as just VehicleState<float64_t>{} and VehicleCommand<float64_t>{}.

◆ check_collision_bounding_box_vs_obstacles()

static bool autoware::motion::planning::parking_planner::check_collision_bounding_box_vs_obstacles	(	const VehicleState< float64_t > &	vehicle_state,
		const Polytope2D< float64_t > &	bounding_box,
		const std::vector< Polytope2D< float64_t >> &	obstacles
	)

static

◆ convert_drivable_area_to_obstacles()

std::vector< Polytope2D< float64_t > > autoware::motion::planning::parking_planner::convert_drivable_area_to_obstacles ( const lanelet::Polygon3d & drivable_area )

◆ convert_parking_planner_to_autoware_trajectory()

autoware_auto_msgs::msg::Trajectory autoware::motion::planning::parking_planner::convert_parking_planner_to_autoware_trajectory ( const Trajectory< float64_t > & parking_trajectory )

◆ create_constraint_bounds()

ConstraintBounds autoware::motion::planning::parking_planner::create_constraint_bounds ( )

◆ create_dummy_nlpobstacle()

static NLPObstacle<double> autoware::motion::planning::parking_planner::create_dummy_nlpobstacle	(	const double	lambda_guess,
		const double	mu_guess,
		const VehicleState< double > &	current_state
	)

static

◆ create_nlpobstacle()

static NLPObstacle<double> autoware::motion::planning::parking_planner::create_nlpobstacle	(	const Polytope2D< double >	polytope,
		const double	lambda_guess,
		const double	mu_guess
	)

static

◆ create_obstacles_from_polyhedra()

std::vector<NLPObstacle<double> > autoware::motion::planning::parking_planner::create_obstacles_from_polyhedra	(	const std::vector< Polytope2D< double >> &	obstacles,
		const VehicleState< double > &	current_state
	)

◆ disassemble_variable_vector()

template<typename T >

Trajectory<float64_t> autoware::motion::planning::parking_planner::disassemble_variable_vector	(	const std::vector< T > &	variable_vector,
		const std::size_t	horizon_length
	)

Helper function to turn a serialized variable vector into a structured Trajectory object.

◆ expand_state_longitudinal_with_heading()

static std::vector<VehicleState<float64_t> > autoware::motion::planning::parking_planner::expand_state_longitudinal_with_heading ( const VehicleState< float64_t > & from_state )

static

◆ get_outer_boxes()

template<typename Iter >

static std::vector<std::list<Point> > autoware::motion::planning::parking_planner::get_outer_boxes	(	const lanelet::Polygon3d &	drivable_area,
		const Iter	convex_hull_start,
		const Iter	convex_hull_end
	)

static

◆ get_pocket_hulls()

template<typename Iter1 , typename Iter2 >

static std::vector<std::list<Point> > autoware::motion::planning::parking_planner::get_pocket_hulls	(	const Iter1	polygon_start,
		const Iter1	polygon_end,
		const Iter2	convex_hull_start,
		const Iter2	convex_hull_end
	)

static

◆ lanelet_point_to_point()

static Point autoware::motion::planning::parking_planner::lanelet_point_to_point ( const lanelet::ConstPoint3d & lanelet_point )

static

◆ make_nlpobstacle_stage_variables()

static std::vector<NLPObstacleStageVariables<double> > autoware::motion::planning::parking_planner::make_nlpobstacle_stage_variables	(	const double	lambda_guess,
		const double	mu_guess
	)

static

◆ map_state_on_discretized_grid()

static uint64_t autoware::motion::planning::parking_planner::map_state_on_discretized_grid	(	const VehicleState< float64_t > &	state,
		const VehicleState< float64_t > &	reference
	)

static

◆ operator*()

template<typename T >

std::vector<T> autoware::motion::planning::parking_planner::operator*	(	const T &	scalar,
		const std::vector< T > &	vector
	)

◆ operator+()

template<typename T >

std::vector<T> autoware::motion::planning::parking_planner::operator+	(	const std::vector< T > &	vector1,
		const std::vector< T > &	vector2
	)

◆ resize_state_vector()

static std::vector<VehicleState<float64_t> > autoware::motion::planning::parking_planner::resize_state_vector	(	const std::vector< VehicleState< float64_t >> &	states_input,
		const std::size_t	target_length
	)

static

◆ RK4() [1/2]

template<class X , class U , typename P >

X autoware::motion::planning::parking_planner::RK4	(	X	x,
		U	u,
		P	p,
		std::function< X(X, U, P)>	f,
		const float64_t	stepsize,
		const std::size_t	steps
	)

Template for an explicit Runge-Kutta integrator. CasADi has its own, but it for some reason doesn't compose with the IPOPT solver generator :shrug:

◆ RK4() [2/2]

template<class X , class U >

X autoware::motion::planning::parking_planner::RK4	(	X	x,
		U	u,
		std::function< X(X, U)>	f,
		const float64_t	stepsize,
		const std::size_t	steps
	)

Variable Documentation

◆ ARTIFICIAL_DUAL_MULTIPLIER_BOUND

constexpr auto autoware::motion::planning::parking_planner::ARTIFICIAL_DUAL_MULTIPLIER_BOUND = 1.0e4

Since we need upper and lower bounds in the constructs used here, but some variables do not have clearly defined upper bounds, we introduce an artificial upper bound number that is "large enough to never matter". The value has been determined experimentally to work - picking this value too small will lead to the problem no longer being mathematically correct, making it too large will lead to numerical difficulties.

◆ DELTA_HEADING

const float64_t autoware::motion::planning::parking_planner::DELTA_HEADING = MY_PI / 24.0

static

◆ DELTA_LONGITUDINAL

constexpr float64_t autoware::motion::planning::parking_planner::DELTA_LONGITUDINAL = 0.125

static

◆ DUMMY_OBSTACLE_DISTANCE

constexpr auto autoware::motion::planning::parking_planner::DUMMY_OBSTACLE_DISTANCE = 2e2

◆ HORIZON_LENGTH

constexpr std::size_t autoware::motion::planning::parking_planner::HORIZON_LENGTH = 30

NLP horizon length, roughly proportional to the maximum useful trajectory length.

◆ INTEGRATION_STEP_SIZE

constexpr float64_t autoware::motion::planning::parking_planner::INTEGRATION_STEP_SIZE = 0.3

Step size (in seconds) for the RK4 integration.

◆ LARGE_NEGATIVE_NUMBER

constexpr auto autoware::motion::planning::parking_planner::LARGE_NEGATIVE_NUMBER = -1.0e4

◆ MAX_EGO_HYPERPLANES

constexpr std::size_t autoware::motion::planning::parking_planner::MAX_EGO_HYPERPLANES = 4

Maximum number of ego vehicle hyperplanes supported by the NLP planner.

◆ MAX_EXPLORATION_RADIUS

constexpr float64_t autoware::motion::planning::parking_planner::MAX_EXPLORATION_RADIUS = 30.0

static

◆ MAX_HYPERPLANES_PER_OBSTACLE

constexpr std::size_t autoware::motion::planning::parking_planner::MAX_HYPERPLANES_PER_OBSTACLE = 5

Maximum number of hyperplanes per obstacle supported by the NLP planner.

◆ MAX_NUM_EXPLORATION_NODES

constexpr size_t autoware::motion::planning::parking_planner::MAX_NUM_EXPLORATION_NODES = 1000000

static

◆ MAX_NUMBER_OF_OBSTACLES

constexpr std::size_t autoware::motion::planning::parking_planner::MAX_NUMBER_OF_OBSTACLES = 10

Maximum number of obstacles supported by the NLP planner.

◆ MY_PI

constexpr float64_t autoware::motion::planning::parking_planner::MY_PI = 3.141592653589793

static

◆ NUMBER_OF_INTEGRATION_STEPS

constexpr std::size_t autoware::motion::planning::parking_planner::NUMBER_OF_INTEGRATION_STEPS = 4

Number of (sub-)steps performed INTEGRATION_STEP_SIZE for the RK4 integration.

Classes

Typedefs

Enumerations

Functions

Variables

Detailed Description

Typedef Documentation

◆ Point

◆ Trajectory

◆ VehicleStateDerivative

Enumeration Type Documentation

◆ PlanningStatus

Function Documentation

◆ are_points_equal()

◆ are_vectors_close()

◆ assemble_parameter_vector()

◆ assemble_variable_vector_and_bounds()

◆ check_collision_bounding_box_vs_obstacles()

◆ convert_drivable_area_to_obstacles()

◆ convert_parking_planner_to_autoware_trajectory()

◆ create_constraint_bounds()

◆ create_dummy_nlpobstacle()

◆ create_nlpobstacle()

◆ create_obstacles_from_polyhedra()

◆ disassemble_variable_vector()

◆ expand_state_longitudinal_with_heading()

◆ get_outer_boxes()

◆ get_pocket_hulls()

◆ lanelet_point_to_point()

◆ make_nlpobstacle_stage_variables()

◆ map_state_on_discretized_grid()

◆ operator*()

◆ operator+()

◆ resize_state_vector()

◆ RK4() [1/2]

◆ RK4() [2/2]

Variable Documentation

◆ ARTIFICIAL_DUAL_MULTIPLIER_BOUND

◆ DELTA_HEADING

◆ DELTA_LONGITUDINAL

◆ DUMMY_OBSTACLE_DISTANCE

◆ HORIZON_LENGTH

◆ INTEGRATION_STEP_SIZE

◆ LARGE_NEGATIVE_NUMBER

◆ MAX_EGO_HYPERPLANES

◆ MAX_EXPLORATION_RADIUS

◆ MAX_HYPERPLANES_PER_OBSTACLE

◆ MAX_NUM_EXPLORATION_NODES

◆ MAX_NUMBER_OF_OBSTACLES

◆ MY_PI

◆ NUMBER_OF_INTEGRATION_STEPS