Motion primitive planner in voxel map. More...

#include <map_planner.h>

Inheritance diagram for MPL::MapPlanner< Dim >:

Public Member Functions
	MapPlanner (bool verbose)
	Simple constructor. More...

virtual void	setMapUtil (const std::shared_ptr< MapUtil< Dim >> &map_util)
	Set map util.

void	setSearchRegion (const vec_Vecf< Dim > &path, bool dense=false)
	Set search region. More...

void	setSearchRadius (const Vecf< Dim > &radius)
	Set search radius (tunnel radius)

void	setPotentialRadius (const Vecf< Dim > &radius)
	Set potential radius.

void	setPotentialMapRange (const Vecf< Dim > &range)
	Set potential map size.

void	setGradientWeight (decimal_t w)
	Set gradient weight.

void	setPotentialWeight (decimal_t w)
	Set potential weight.

vec_Vec3f	getPotentialCloud (decimal_t h_max=1.0)
	Get the potential cloud, works for 2D and 3D.

vec_Vec3f	getGradientCloud (decimal_t h_max=1.0, int i=0)
	Get the gradient cloud, works for 2D.

vec_Vecf< Dim >	getSearchRegion () const
	Get search region.

vec_Vecf< Dim >	getLinkedNodes () const
	Get linked voxels.

void	updateBlockedNodes (const vec_Veci< Dim > &pns)
	Update edge costs according to the new blocked nodes. More...

void	updateClearedNodes (const vec_Veci< Dim > &pns)
	Update edge costs according to the new cleared nodes. More...

void	updatePotentialMap (const Vecf< Dim > &pos)
	Generate potential map. More...

bool	iterativePlan (const Waypoint< Dim > &start, const Waypoint< Dim > &goal, const Trajectory< Dim > &raw_traj, int max_iter_num=3)
	Iterative trajectory planning with APFs. More...

Public Member Functions inherited from MPL::PlannerBase< Dim, Waypoint< Dim > >
	PlannerBase (bool verbose=false)
	Simple constructor.

bool	initialized ()
	Check if the planner has been initialized.

Trajectory< Dim >	getTraj () const
	Get optimal trajectory.

vec_E< Primitive< Dim > >	getValidPrimitives () const
	Get expanded collision free primitives.

vec_E< Primitive< Dim > >	getAllPrimitives () const
	Get expanded primitives.

vec_Vecf< Dim >	getOpenSet () const
	Get points in open set.

vec_Vecf< Dim >	getCloseSet () const
	Get points in close set.

vec_Vecf< Dim >	getNullSet () const
	Get points neither in open nor close set.

vec_Vecf< Dim >	getStates (const Waypoint< Dim > &state) const
	Get points at certain state.

vec_Vecf< Dim >	getExpandedNodes () const
	Get expanded nodes, for A* it should be the same as the close set.

vec_E< Primitive< Dim > >	getExpandedEdges () const
	Get expanded edges, for A* it should be the same as the close set.

int	getExpandedNum () const
	Get number of expanded nodes.

void	getSubStateSpace (int time_step)
	Prune state space. More...

decimal_t	getTrajCost () const
	Get trajectory total cost.

void	checkValidation ()
	Check tree validation.

void	reset ()
	Reset state space.

void	setLPAstar (bool use_lpastar)
	Set max vel in each axis.

void	setVmax (decimal_t v)
	Set max vel in each axis.

void	setAmax (decimal_t a)
	Set max acc in each axis.

void	setJmax (decimal_t j)
	Set max jerk in each axis.

void	setYawmax (decimal_t yaw)
	Set max jerk in each axis.

void	setTmax (decimal_t t)
	Set max time step to explore.

void	setDt (decimal_t dt)
	Set dt for each primitive.

void	setW (decimal_t w)
	Set weight for cost in time.

void	setWyaw (decimal_t w)
	Set weight for cost in time.

void	setEpsilon (decimal_t eps)
	Set greedy searching param.

void	setHeurIgnoreDynamics (bool ignore)
	Calculate heuristic using dynamics.

void	setMaxNum (int num)
	Set max number of expansion.

void	setU (const vec_E< VecDf > &U)
	Set U.

void	setPriorTrajectory (const Trajectory< Dim > &traj)
	Set prior trajectory.

void	setTol (decimal_t tol_pos, decimal_t tol_vel=-1, decimal_t tol_acc=-1)
	Set tolerance in geometric and dynamic spaces.

bool	plan (const Waypoint< Dim > &start, const Waypoint< Dim > &goal)
	Planning thread. More...

Protected Member Functions
void	createMask ()
	Create mask for potential.

vec_E< Vecf< Dim > >	calculateGradient (const Veci< Dim > &coord1, const Veci< Dim > &coord2)
	Calculate local gradient map.

Protected Attributes
std::shared_ptr< MapUtil< Dim > >	map_util_
	Map util.

linkedHashMap< Dim >	lhm_

int8_t	H_MAX {100}
	Max value for potential.

Vecf< Dim >	potential_radius_ {Vecf<Dim>::Zero()}
	Radius of potential for each voxel.

Vecf< Dim >	potential_map_range_ {Vecf<Dim>::Zero()}
	Potential map size, centered at the given pos.

vec_E< std::pair< Veci< Dim >, int8_t > >	potential_mask_
	Mask for generating potential field around obstacle.

decimal_t	pow_ {1.0}
	Power of potential value.

vec_E< Vecf< Dim > >	gradient_map_
	Gradient map.

Vecf< Dim >	search_radius_ {Vecf<Dim>::Zero()}
	Search radius (tunnel size)

Protected Attributes inherited from MPL::PlannerBase< Dim, Waypoint< Dim > >
std::shared_ptr< MPL::env_base< Dim > >	ENV_
	Environment class.

std::shared_ptr< MPL::StateSpace< Dim, Waypoint< Dim > > >	ss_ptr_
	Planner workspace.

Trajectory< Dim >	traj_
	Optimal trajectory.

decimal_t	traj_cost_
	Total cost of final trajectory.

decimal_t	epsilon_
	Greedy searching parameter.

int	max_num_
	Maxmum number of expansion, -1 means no limitation.

bool	use_lpastar_
	Enable LPAstar for planning.

bool	planner_verbose_
	Enabled to display debug message.

Detailed Description

template<int Dim>
class MPL::MapPlanner< Dim >

Motion primitive planner in voxel map.

Constructor & Destructor Documentation

◆ MapPlanner()

template<int Dim>

MapPlanner::MapPlanner ( bool verbose )

Simple constructor.

Parameters

verbose enable debug messages

Member Function Documentation

◆ iterativePlan()

template<int Dim>

bool MapPlanner::iterativePlan	(	const Waypoint< Dim > &	start,
		const Waypoint< Dim > &	goal,
		const Trajectory< Dim > &	raw_traj,
		int	max_iter_num = `3`
	)

Iterative trajectory planning with APFs.

Parameters

start	start waypoint
goal	goal waypoint
raw_traj	the trajectory to be perturbed
max_iter_num	number of max iterations, default value is 3

◆ setSearchRegion()

template<int Dim>

void MapPlanner::setSearchRegion	(	const vec_Vecf< Dim > &	path,
		bool	dense = `false`
	)

Set search region.

Parameters

path	a sequence of waypoints from a path or trajectory
dense	if true, do ray cast between two consecutive points in path

◆ updateBlockedNodes()

template<int Dim>

void MapPlanner::updateBlockedNodes ( const vec_Veci< Dim > & pns )

Update edge costs according to the new blocked nodes.

Parameters

pns	the new occupied voxels

The function returns affected primitives for debug purpose

◆ updateClearedNodes()

template<int Dim>

void MapPlanner::updateClearedNodes ( const vec_Veci< Dim > & pns )

Update edge costs according to the new cleared nodes.

Parameters

pns	the new cleared voxels

The function returns affected primitives for debug purpose

◆ updatePotentialMap()

template<int Dim>

void MapPlanner::updatePotentialMap ( const Vecf< Dim > & pos )

Generate potential map.

Parameters

pos	center of the potential map range is zero, do global generation
pow	power of potential field

Member Data Documentation

◆ lhm_

template<int Dim>

linkedHashMap<Dim> MPL::MapPlanner< Dim >::lhm_

mutableprotected

Linked table that records voxel and corresponding primitives passed through

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

include/mpl_planner/planner/map_planner.h
src/mpl_planner/map_planner.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<int Dim> class MPL::MapPlanner< Dim >

Constructor & Destructor Documentation

◆ MapPlanner()

Member Function Documentation

◆ iterativePlan()

◆ setSearchRegion()

◆ updateBlockedNodes()

◆ updateClearedNodes()

◆ updatePotentialMap()

Member Data Documentation

◆ lhm_

template<int Dim>
class MPL::MapPlanner< Dim >