#include <Planner.hpp>

Collaboration diagram for ugv_nav4d::Planner:

Public Types
enum	PLANNING_RESULT { GOAL_INVALID , START_INVALID , NO_SOLUTION , NO_MAP , INTERNAL_ERROR , FOUND_SOLUTION }

Public Member Functions
	Planner (const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig, const traversability_generator3d::TraversabilityConfig &traversabilityConfig, const Mobility &mobility, const PlannerConfig &plannerConfig)

template<maps::grid::MLSConfig::update_model SurfacePatch>
void	updateMap (const maps::grid::MLSMap< SurfacePatch > &mls)

void	updateMap (const MLSBase &mls)

void	setInitialPatch (const Eigen::Affine3d &body2Mls, double patchRadius)

void	enablePathStatistics (bool enable)

void	setTravMapCallback (const std::function< void()> &callback)

std::vector< Motion >	getMotions () const

PLANNING_RESULT	plan (const base::Time &maxTime, const base::samples::RigidBodyState &start_pose, const base::samples::RigidBodyState &end_pose, std::vector< trajectory_follower::SubTrajectory > &resultTrajectory2D, std::vector< trajectory_follower::SubTrajectory > &resultTrajectory3D, bool dumpOnError=false, bool dumpOnSuccess=false)

void	setTravConfig (const traversability_generator3d::TraversabilityConfig &config)

void	setPlannerConfig (const PlannerConfig &config)

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > &	getTraversabilityMap () const

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > &	getObstacleMap () const

std::shared_ptr< trajectory_follower::SubTrajectory >	findTrajectoryOutOfObstacle (const Eigen::Vector3d &start, double theta, const Eigen::Affine3d &ground2Body)

Protected Types
typedef EnvironmentXYZTheta::MLGrid	MLSBase

Protected Attributes
std::shared_ptr< EnvironmentXYZTheta >	env

std::shared_ptr< ARAPlanner >	planner

const sbpl_spline_primitives::SplinePrimitivesConfig	splinePrimitiveConfig

const Mobility	mobility

traversability_generator3d::TraversabilityConfig	traversabilityConfig

PlannerConfig	plannerConfig

std::vector< int >	solutionIds

std::function< void()>	travMapCallback

std::vector< Eigen::Vector3d >	previousStartPositions

Friends
class	PlannerDump

Member Typedef Documentation

◆ MLSBase

typedef EnvironmentXYZTheta::MLGrid ugv_nav4d::Planner::MLSBase

protected

Member Enumeration Documentation

◆ PLANNING_RESULT

enum ugv_nav4d::Planner::PLANNING_RESULT

Enumerator
GOAL_INVALID
START_INVALID
NO_SOLUTION	Happens if the planner runs out of time or the complete state space has been explored without a solution
NO_MAP
INTERNAL_ERROR
FOUND_SOLUTION

Constructor & Destructor Documentation

◆ Planner()

ugv_nav4d::Planner::Planner	(	const sbpl_spline_primitives::SplinePrimitivesConfig &	primitiveConfig,
		const traversability_generator3d::TraversabilityConfig &	traversabilityConfig,
		const Mobility &	mobility,
		const PlannerConfig &	plannerConfig
	)

Here is the call graph for this function:

Member Function Documentation

◆ enablePathStatistics()

void ugv_nav4d::Planner::enablePathStatistics ( bool enable )

◆ findTrajectoryOutOfObstacle()

std::shared_ptr< SubTrajectory > ugv_nav4d::Planner::findTrajectoryOutOfObstacle	(	const Eigen::Vector3d &	start,
		double	theta,
		const Eigen::Affine3d &	ground2Body
	)

◆ getMotions()

std::vector< Motion > ugv_nav4d::Planner::getMotions ( ) const

◆ getObstacleMap()

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > & ugv_nav4d::Planner::getObstacleMap ( ) const

◆ getTraversabilityMap()

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > & ugv_nav4d::Planner::getTraversabilityMap ( ) const

◆ plan()

Planner::PLANNING_RESULT ugv_nav4d::Planner::plan	(	const base::Time &	maxTime,
		const base::samples::RigidBodyState &	start_pose,
		const base::samples::RigidBodyState &	end_pose,
		std::vector< trajectory_follower::SubTrajectory > &	resultTrajectory2D,
		std::vector< trajectory_follower::SubTrajectory > &	resultTrajectory3D,
		bool	dumpOnError = `false`,
		bool	dumpOnSuccess = `false`
	)

Plan a path from start to end.

This will expand the map if it has not already been expanded.

This function remembers the start positions from previous calls. Those positions will be used (in addition to the current start position) to try to expand the map. Thus even if the current start position is invalid (e.g. inside an obstacle) the map will still be expanded based on the previous start positions. This feature improves the general robustness during planning and generally results in more complete maps under real-world conditions.

Parameters

maxTime	Maximum processor time to use.
startbody2Mls	The start position of the body in mls coordinates. This should be the location of the body-frame. The planner assumes that this location is config.distToGround meters above (!!!) the map. The planner will transform this location to the ground frame using config.distToGround. If this location is not exactly config.distToGround meters above the map, the planner will fail to find the patch that the robot is standing on.
endbody2Mls	The goal position of the body in mls coordinates. See `startbody2Mls` for more details.
resultTrajectory2D	The resulting trajectory without z-coordinates (all z-values are set to zero). This trajectory exists to avoid a bug in spline interpolation. In certain corner cases (when the z change between two steps is much greater than the xy change) the spline interpolator will fit an S-curve instead of a straight line between the two patches. This results in an increase-decrease-increase pattern in xy-direction resulting in a robot motion that stutters. Setting the z-axis to zero was choosen as a fix because the trajectory follower (at the time of writing) ignores the z-axis anyway. A ticket for this bug exists: https://git.hb.dfki.de/entern/ugv_nav4d/issues/1
resultTrajectory3D	The resulting trajectory. Make sure to read the comment for `resultTrajectory2D` to understand why this exists!
dumpOnError	If true a planner dump will be written in case of error. This dump can be loaded and analyzed later using the ugv_nav4d_replay tool.
dumpOnSuccess	If true a planner dump will be written in case of successful planning. This dump can be loaded and analyzed later using the ugv_nav4d_replay tool.

Returns: An enum indicating the planner state

◆ setInitialPatch()

void ugv_nav4d::Planner::setInitialPatch	(	const Eigen::Affine3d &	body2Mls,
		double	patchRadius
	)

◆ setPlannerConfig()

void ugv_nav4d::Planner::setPlannerConfig ( const PlannerConfig & config )

◆ setTravConfig()

void ugv_nav4d::Planner::setTravConfig ( const traversability_generator3d::TraversabilityConfig & config )

Here is the caller graph for this function:

◆ setTravMapCallback()

void ugv_nav4d::Planner::setTravMapCallback ( const std::function< void()> & callback )

This callback is executed, whenever a new traverability map was expanded

◆ updateMap() [1/2]

template<maps::grid::MLSConfig::update_model SurfacePatch>

void ugv_nav4d::Planner::updateMap ( const maps::grid::MLSMap< SurfacePatch > & mls )

inline

◆ updateMap() [2/2]

void ugv_nav4d::Planner::updateMap ( const MLSBase & mls )

inline

Friends And Related Function Documentation

◆ PlannerDump

friend class PlannerDump

friend

Member Data Documentation

◆ env

std::shared_ptr<EnvironmentXYZTheta> ugv_nav4d::Planner::env

protected

◆ mobility

const Mobility ugv_nav4d::Planner::mobility

protected

◆ planner

std::shared_ptr<ARAPlanner> ugv_nav4d::Planner::planner

protected

◆ plannerConfig

PlannerConfig ugv_nav4d::Planner::plannerConfig

protected

◆ previousStartPositions

std::vector<Eigen::Vector3d> ugv_nav4d::Planner::previousStartPositions

protected

are buffered and reused for a more robust map generation

◆ solutionIds

std::vector<int> ugv_nav4d::Planner::solutionIds

protected

◆ splinePrimitiveConfig

const sbpl_spline_primitives::SplinePrimitivesConfig ugv_nav4d::Planner::splinePrimitiveConfig

protected

◆ traversabilityConfig

traversability_generator3d::TraversabilityConfig ugv_nav4d::Planner::traversabilityConfig

protected

◆ travMapCallback

std::function<void ()> ugv_nav4d::Planner::travMapCallback

protected

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

src/Planner.hpp
src/Planner.cpp

Public Types

Public Member Functions

Protected Types

Protected Attributes

Friends

Member Typedef Documentation

◆ MLSBase

Member Enumeration Documentation

◆ PLANNING_RESULT

Constructor & Destructor Documentation

◆ Planner()

Member Function Documentation

◆ enablePathStatistics()

◆ findTrajectoryOutOfObstacle()

◆ getMotions()

◆ getObstacleMap()

◆ getTraversabilityMap()

◆ plan()

◆ setInitialPatch()

◆ setPlannerConfig()

◆ setTravConfig()

◆ setTravMapCallback()

◆ updateMap() [1/2]

◆ updateMap() [2/2]

Friends And Related Function Documentation

◆ PlannerDump

Member Data Documentation

◆ env

◆ mobility

◆ planner

◆ plannerConfig

◆ previousStartPositions

◆ solutionIds

◆ splinePrimitiveConfig

◆ traversabilityConfig

◆ travMapCallback