#include <mpp/ptgs/DiffDriveCollisionGridBased.h>

Inheritance diagram for mpp::ptg::DiffDriveCollisionGridBased:

Classes
class	CCollisionGrid

struct	TCellForLambdaFunction

Public Member Functions
virtual void	ptgDiffDriveSteeringFunction (float alpha, float t, float x, float y, float phi, float &v, float &w) const =0

double	getMax_V () const

double	getMax_W () const

Virtual interface of each PTG implementation
bool	inverseMap_WS2TP (double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const override

mrpt::kinematics::CVehicleVelCmd::Ptr	directionToMotionCommand (uint16_t k) const override

mrpt::kinematics::CVehicleVelCmd::Ptr	getSupportedKinematicVelocityCommand () const override

void	setRefDistance (const double refDist) override

size_t	getPathStepCount (uint16_t k) const override

mrpt::math::TPose2D	getPathPose (uint16_t k, uint32_t step) const override

double	getPathDist (uint16_t k, uint32_t step) const override

bool	getPathStepForDist (uint16_t k, double dist, uint32_t &out_step) const override

double	getPathStepDuration () const override

double	getMaxLinVel () const override

double	getMaxAngVel () const override

void	updateTPObstacle (double ox, double oy, std::vector< double > &tp_obstacles) const override

void	updateTPObstacleSingle (double ox, double oy, uint16_t k, double &tp_obstacle_k) const override

void	onNewNavDynamicState () override

Protected Types
using	TCollisionCell = std::vector< std::pair< uint16_t, float > >

Protected Member Functions
	DiffDriveCollisionGridBased ()

void	internal_processNewRobotShape () override

void	internal_initialize (const std::string &cacheFilename=std::string(), const bool verbose=true) override

void	internal_deinitialize () override

void	loadFromConfigFile (const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override

void	saveToConfigFile (mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override

void	loadDefaultParams () override

void	internal_readFromStream (mrpt::serialization::CArchive &in) override

void	internal_writeToStream (mrpt::serialization::CArchive &out) const override

void	simulateTrajectories (float max_time, float max_dist, float dt)

mrpt::math::TTwist2D	getPathTwist (uint16_t k, uint32_t step) const override

bool	saveColGridsToFile (const std::string &filename, const mrpt::math::CPolygon &computed_robotShape) const

bool	loadColGridsFromFile (const std::string &filename, const mrpt::math::CPolygon &current_robotShape)

Protected Attributes
double	V_MAX {.0}

double	W_MAX {.0}

double	turningRadiusReference {.10}

std::vector< TCPointVector >	m_trajectory

double	m_resolution {0.05}

double	m_stepTimeDuration {0.01}

CCollisionGrid	m_collisionGrid

mrpt::containers::CDynamicGrid< TCellForLambdaFunction >	m_lambdaFunctionOptimizer

Detailed Description

Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles based on numerical integration of the trajectories and collision look-up-table. Regarding initialize(): in this this family of PTGs, the method builds the collision grid or load it from a cache file. Collision grids must be calculated before calling getTPObstacle(). Robot shape must be set before initializing with setRobotShape(). The rest of PTG parameters should have been set at the constructor.

Note: This class is forked from mrpt::nav to expose more parameters related to the path simulation time steps.

Member Typedef Documentation

◆ TCollisionCell

using mpp::ptg::DiffDriveCollisionGridBased::TCollisionCell = std::vector<std::pair<uint16_t, float> >

protected

A list of all the pairs (alpha,distance) such as the robot collides at that cell.

map key (uint16_t) -> alpha value (k)
- map value (float) -> the MINIMUM distance (d), in meters, associated with that "k".

Constructor & Destructor Documentation

◆ DiffDriveCollisionGridBased()

mpp::ptg::DiffDriveCollisionGridBased::DiffDriveCollisionGridBased ( )

protected

Member Function Documentation

◆ directionToMotionCommand()

mrpt::kinematics::CVehicleVelCmd::Ptr mpp::ptg::DiffDriveCollisionGridBased::directionToMotionCommand ( uint16_t k ) const

override

In this class, out_action_cmd contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s). See more docs in CParameterizedTrajectoryGenerator::directionToMotionCommand()

◆ getMax_V()

double mpp::ptg::DiffDriveCollisionGridBased::getMax_V ( ) const

inline

◆ getMax_W()

double mpp::ptg::DiffDriveCollisionGridBased::getMax_W ( ) const

inline

◆ getMaxAngVel()

double mpp::ptg::DiffDriveCollisionGridBased::getMaxAngVel ( ) const

inlineoverride

◆ getMaxLinVel()

double mpp::ptg::DiffDriveCollisionGridBased::getMaxLinVel ( ) const

inlineoverride

◆ getPathDist()

double mpp::ptg::DiffDriveCollisionGridBased::getPathDist	(	uint16_t	k,
		uint32_t	step
	)		const

override

◆ getPathPose()

mrpt::math::TPose2D mpp::ptg::DiffDriveCollisionGridBased::getPathPose	(	uint16_t	k,
		uint32_t	step
	)		const

override

◆ getPathStepCount()

size_t mpp::ptg::DiffDriveCollisionGridBased::getPathStepCount ( uint16_t k ) const

override

◆ getPathStepDuration()

double mpp::ptg::DiffDriveCollisionGridBased::getPathStepDuration ( ) const

override

◆ getPathStepForDist()

bool mpp::ptg::DiffDriveCollisionGridBased::getPathStepForDist	(	uint16_t	k,
		double	dist,
		uint32_t &	out_step
	)		const

override

◆ getPathTwist()

mrpt::math::TTwist2D mpp::ptg::DiffDriveCollisionGridBased::getPathTwist	(	uint16_t	k,
		uint32_t	step
	)		const

overrideprotected

◆ getSupportedKinematicVelocityCommand()

mrpt::kinematics::CVehicleVelCmd::Ptr mpp::ptg::DiffDriveCollisionGridBased::getSupportedKinematicVelocityCommand ( ) const

override

◆ internal_deinitialize()

void mpp::ptg::DiffDriveCollisionGridBased::internal_deinitialize ( )

overrideprotected

◆ internal_initialize()

void mpp::ptg::DiffDriveCollisionGridBased::internal_initialize	(	const std::string &	cacheFilename = `std::string()`,
		const bool	verbose = `true`
	)

overrideprotected

◆ internal_processNewRobotShape()

void mpp::ptg::DiffDriveCollisionGridBased::internal_processNewRobotShape ( )

overrideprotected

◆ internal_readFromStream()

void mpp::ptg::DiffDriveCollisionGridBased::internal_readFromStream ( mrpt::serialization::CArchive & in )

overrideprotected

◆ internal_writeToStream()

void mpp::ptg::DiffDriveCollisionGridBased::internal_writeToStream ( mrpt::serialization::CArchive & out ) const

overrideprotected

◆ inverseMap_WS2TP()

bool mpp::ptg::DiffDriveCollisionGridBased::inverseMap_WS2TP	(	double	x,
		double	y,
		int &	out_k,
		double &	out_d,
		double	tolerance_dist = `0.10`
	)		const

override

The default implementation in this class relies on a look-up-table. Derived classes may redefine this to closed-form expressions, when they exist. See full docs in base class CParameterizedTrajectoryGenerator::inverseMap_WS2TP()

◆ loadColGridsFromFile()

bool mpp::ptg::DiffDriveCollisionGridBased::loadColGridsFromFile	(	const std::string &	filename,
		const mrpt::math::CPolygon &	current_robotShape
	)

protected

◆ loadDefaultParams()

void mpp::ptg::DiffDriveCollisionGridBased::loadDefaultParams ( )

overrideprotected

◆ loadFromConfigFile()

void mpp::ptg::DiffDriveCollisionGridBased::loadFromConfigFile	(	const mrpt::config::CConfigFileBase &	cfg,
		const std::string &	sSection
	)

overrideprotected

Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally, plus):

${sKeyPrefix}resolution: The cell size
${sKeyPrefix}v_max, ${sKeyPrefix}w_max`: Maximum robot speeds.
`${sKeyPrefix}shape_x{0,1,2..}`,${sKeyPrefix}shape_y{0,1,2..}`: Polygonal robot shape [Optional, can be also set via setRobotPolygonShape()]

See docs of derived classes for additional parameters in setParams()

◆ onNewNavDynamicState()

void mpp::ptg::DiffDriveCollisionGridBased::onNewNavDynamicState ( )

inlineoverride

This family of PTGs ignores the dynamic states

◆ ptgDiffDriveSteeringFunction()

virtual void mpp::ptg::DiffDriveCollisionGridBased::ptgDiffDriveSteeringFunction	(	float	alpha,
		float	t,
		float	x,
		float	y,
		float	phi,
		float &	v,
		float &	w
	)		const

pure virtual

The main method to be implemented in derived classes: it defines the differential-driven differential equation

Implemented in mpp::ptg::DiffDrive_C.

◆ saveColGridsToFile()

bool mpp::ptg::DiffDriveCollisionGridBased::saveColGridsToFile	(	const std::string &	filename,
		const mrpt::math::CPolygon &	computed_robotShape
	)		const

protected

◆ saveToConfigFile()

void mpp::ptg::DiffDriveCollisionGridBased::saveToConfigFile	(	mrpt::config::CConfigFileBase &	cfg,
		const std::string &	sSection
	)		const

overrideprotected

◆ setRefDistance()

void mpp::ptg::DiffDriveCollisionGridBased::setRefDistance ( const double refDist )

override

Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change the reference distance after initialization.

◆ simulateTrajectories()

void mpp::ptg::DiffDriveCollisionGridBased::simulateTrajectories	(	float	max_time,
		float	max_dist,
		float	dt
	)

protected

Numerically solve the diferential equations to generate a family of trajectories

◆ updateTPObstacle()

void mpp::ptg::DiffDriveCollisionGridBased::updateTPObstacle	(	double	ox,
		double	oy,
		std::vector< double > &	tp_obstacles
	)		const

override

◆ updateTPObstacleSingle()

void mpp::ptg::DiffDriveCollisionGridBased::updateTPObstacleSingle	(	double	ox,
		double	oy,
		uint16_t	k,
		double &	tp_obstacle_k
	)		const

override

Member Data Documentation

◆ m_collisionGrid

CCollisionGrid mpp::ptg::DiffDriveCollisionGridBased::m_collisionGrid

protected

The collision grid

◆ m_lambdaFunctionOptimizer

mrpt::containers::CDynamicGrid<TCellForLambdaFunction> mpp::ptg::DiffDriveCollisionGridBased::m_lambdaFunctionOptimizer

protected

This grid will contain indexes data for speeding-up the default, brute-force lambda function

◆ m_resolution

double mpp::ptg::DiffDriveCollisionGridBased::m_resolution {0.05}

protected

◆ m_stepTimeDuration

double mpp::ptg::DiffDriveCollisionGridBased::m_stepTimeDuration {0.01}

protected

◆ m_trajectory

std::vector<TCPointVector> mpp::ptg::DiffDriveCollisionGridBased::m_trajectory

protected

◆ turningRadiusReference

double mpp::ptg::DiffDriveCollisionGridBased::turningRadiusReference {.10}

protected

◆ V_MAX

double mpp::ptg::DiffDriveCollisionGridBased::V_MAX {.0}

protected

◆ W_MAX

double mpp::ptg::DiffDriveCollisionGridBased::W_MAX {.0}

protected

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

/home/docs/checkouts/readthedocs.org/user_builds/selfdriving/checkouts/latest/mrpt_path_planning/include/mpp/ptgs/DiffDriveCollisionGridBased.h

Classes

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ TCollisionCell

Constructor & Destructor Documentation

◆ DiffDriveCollisionGridBased()

Member Function Documentation

◆ directionToMotionCommand()

◆ getMax_V()

◆ getMax_W()

◆ getMaxAngVel()

◆ getMaxLinVel()

◆ getPathDist()

◆ getPathPose()

◆ getPathStepCount()

◆ getPathStepDuration()

◆ getPathStepForDist()

◆ getPathTwist()

◆ getSupportedKinematicVelocityCommand()

◆ internal_deinitialize()

◆ internal_initialize()

◆ internal_processNewRobotShape()

◆ internal_readFromStream()

◆ internal_writeToStream()

◆ inverseMap_WS2TP()

◆ loadColGridsFromFile()

◆ loadDefaultParams()

◆ loadFromConfigFile()

◆ onNewNavDynamicState()

◆ ptgDiffDriveSteeringFunction()

◆ saveColGridsToFile()

◆ saveToConfigFile()

◆ setRefDistance()

◆ simulateTrajectories()

◆ updateTPObstacle()

◆ updateTPObstacleSingle()

Member Data Documentation

◆ m_collisionGrid

◆ m_lambdaFunctionOptimizer

◆ m_resolution

◆ m_stepTimeDuration

◆ m_trajectory

◆ turningRadiusReference

◆ V_MAX

◆ W_MAX