Detailed Description

TP Space-based RRT path planning for SE(2) (planar) robots.

This planner algorithm is described in the paper:

M. Bellone, J.L. Blanco, A. Gimenez, "TP-Space RRT: Kinematic path planning of non-holonomic any-shape vehicles", International Journal of Advanced Robotic Systems, 2015.

Typical usage:

mrpt::nav::PlannerRRT_SE2_TPS  planner;
 
// Set or load planner parameters:
//planner.loadConfig( mrpt::config::CConfigFile("config_file.cfg") );
//planner.params.... // See RRTAlgorithmParams
 
// Set RRT end criteria (when to stop searching for a solution)
//planner.end_criteria.... // See RRTEndCriteria
 
planner.initialize();  // Initialize after setting the algorithm parameters
 
// Set up planning problem:
PlannerRRT_SE2_TPS::TPlannerResult planner_result;
PlannerRRT_SE2_TPS::TPlannerInput planner_input;
// Start & goal:
planner_input.start_pose = mrpt::math::TPose2D(XXX,XXX,XXX);
planner_input.goal_pose  = mrpt::math::TPose2D(XXX,XXX,XXX);
// Set obtacles: (...)
// planner_input.obstacles_points ...
// Set workspace bounding box for picking random poses in the RRT algorithm:
planner_input.world_bbox_min = mrpt::math::TPoint2D(XX,YY);
planner_input.world_bbox_max = mrpt::math::TPoint2D(XX,YY);
// Do path planning:
planner.solve( planner_input, planner_result);
// Analyze contents of planner_result...

Changes history:
- 06/MAR/2014: Creation (MB)
- 06/JAN/2015: Refactoring (JLBC)

Todo:: Factorize into more generic path planner classes! //template <class POSE, class MOTIONS>...

Definition at line 68 of file PlannerRRT_SE2_TPS.h.

#include <mrpt/nav/planners/PlannerRRT_SE2_TPS.h>

Inheritance diagram for mrpt::nav::PlannerRRT_SE2_TPS:

Classes
struct	TPlannerInput

struct	TPlannerResult

Public Types
using	node_pose_t = mrpt::math::TPose2D
	The type of poses at nodes. More...

Public Member Functions
	PlannerRRT_SE2_TPS ()
	Constructor. More...

void	loadConfig (const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
	Load all params from a config file source. More...

void	initialize ()
	Must be called after setting all params (see `loadConfig()`) and before calling `solve()` More...

void	solve (const TPlannerInput &pi, TPlannerResult &result)
	The main API entry point: tries to find a planned path from 'goal' to 'target'. More...

mrpt::system::CTimeLogger &	getProfiler ()

const mrpt::nav::TListPTGPtr &	getPTGs () const

template<typename node_pose_t , typename world_limits_t , typename tree_t >
void	renderMoveTree (mrpt::opengl::COpenGLScene &scene, const TPlannerInputTempl< node_pose_t, world_limits_t > &pi, const TPlannerResultTempl< tree_t > &result, const TRenderPlannedPathOptions &options)

Public Attributes
RRTEndCriteria	end_criteria

RRTAlgorithmParams	params
	Parameters specific to this path solver algorithm. More...

Protected Member Functions
void	internal_loadConfig_PTG (const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
	Load all PTG params from a config file source. More...

void	internal_initialize_PTG ()
	Must be called after setting all params (see `internal_loadConfig_PTG()`) and before calling `solve()` More...

void	spaceTransformer (const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< double > &out_TPObstacles)

void	spaceTransformerOneDirectionOnly (const int tp_space_k_direction, const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, double &out_TPObstacle_k)

Static Protected Member Functions
static void	transformPointcloudWithSquareClipping (const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)

Protected Attributes
bool	m_initialized

mrpt::system::CTimeLogger	m_timelogger

bool	m_initialized_PTG

mrpt::nav::TListPTGPtr	m_PTGs

mrpt::maps::CSimplePointsMap	m_local_obs

Member Typedef Documentation

◆ node_pose_t

using mrpt::nav::PlannerRRT_SE2_TPS::node_pose_t = mrpt::math::TPose2D

The type of poses at nodes.

Definition at line 72 of file PlannerRRT_SE2_TPS.h.

Constructor & Destructor Documentation

◆ PlannerRRT_SE2_TPS()

PlannerRRT_SE2_TPS::PlannerRRT_SE2_TPS ( )

Constructor.

Definition at line 26 of file PlannerRRT_SE2_TPS.cpp.

Member Function Documentation

◆ getProfiler()

mrpt::system::CTimeLogger& mrpt::nav::PlannerTPS_VirtualBase::getProfiler ( )

inlineinherited

Definition at line 151 of file PlannerRRT_common.h.

References mrpt::nav::PlannerTPS_VirtualBase::m_timelogger.

◆ getPTGs()

const mrpt::nav::TListPTGPtr& mrpt::nav::PlannerTPS_VirtualBase::getPTGs ( ) const

inlineinherited

Definition at line 152 of file PlannerRRT_common.h.

References mrpt::nav::PlannerTPS_VirtualBase::m_PTGs.

◆ initialize()

void PlannerRRT_SE2_TPS::initialize ( )

Must be called after setting all params (see loadConfig()) and before calling solve()

Definition at line 36 of file PlannerRRT_SE2_TPS.cpp.

References mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG().

◆ internal_initialize_PTG()

void PlannerTPS_VirtualBase::internal_initialize_PTG ( )

protectedinherited

Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve()

Definition at line 38 of file PlannerRRT_common.cpp.

References ASSERTMSG_, mrpt::format(), mrpt::math::TPolygon2D::getPlotData(), mrpt::nav::PlannerTPS_VirtualBase::m_initialized_PTG, mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, mrpt::nav::PlannerTPS_VirtualBase::m_timelogger, mrpt::math::CPolygon::setAllVertices(), mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape(), and mrpt::nav::CPTG_RobotShape_Circular::setRobotShapeRadius().

Referenced by initialize().

◆ internal_loadConfig_PTG()

void PlannerTPS_VirtualBase::internal_loadConfig_PTG	(	const mrpt::config::CConfigFileBase &	cfgSource,
		const std::string &	sSectionName = `std::string("PTG_CONFIG")`
	)

protectedinherited

Load all PTG params from a config file source.

Definition at line 97 of file PlannerRRT_common.cpp.

References ASSERT_, mrpt::nav::CParameterizedTrajectoryGenerator::CreatePTG(), mrpt::format(), mrpt::nav::PlannerTPS_VirtualBase::m_PTGs, mrpt::config::CConfigFileBase::read_double(), mrpt::config::CConfigFileBase::read_int(), mrpt::config::CConfigFileBase::read_string(), and THROW_EXCEPTION_FMT.

Referenced by loadConfig().

◆ loadConfig()

void PlannerRRT_SE2_TPS::loadConfig	(	const mrpt::config::CConfigFileBase &	cfgSource,
		const std::string &	sSectionName = `std::string("PTG_CONFIG")`
	)

Load all params from a config file source.

Definition at line 28 of file PlannerRRT_SE2_TPS.cpp.

References mrpt::nav::PlannerTPS_VirtualBase::internal_loadConfig_PTG().

◆ renderMoveTree()

template<typename node_pose_t , typename world_limits_t , typename tree_t >

void mrpt::nav::PlannerTPS_VirtualBase::renderMoveTree	(	mrpt::opengl::COpenGLScene &	scene,
		const TPlannerInputTempl< node_pose_t, world_limits_t > &	pi,
		const TPlannerResultTempl< tree_t > &	result,
		const TRenderPlannedPathOptions &	options
	)

inherited

Definition at line 25 of file impl_renderMoveTree.h.

◆ solve()

void PlannerRRT_SE2_TPS::solve	(	const TPlannerInput &	pi,
		PlannerRRT_SE2_TPS::TPlannerResult &	result
	)

The main API entry point: tries to find a planned path from 'goal' to 'target'.

Definition at line 45 of file PlannerRRT_SE2_TPS.cpp.

References mrpt::nav::TPlannerResultTempl< tree_t >::acceptable_goal_node_ids, mrpt::math::angDistance(), ASSERT_ABOVE_, ASSERTMSG_, mrpt::poses::CPose2D::asTPose(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::backtrackPath(), mrpt::nav::TPlannerResultTempl< tree_t >::best_goal_node_id, mrpt::nav::TPlannerResultTempl< tree_t >::computation_time, mrpt::nav::TMoveEdgeSE2_TP::cost, mrpt::system::createDirectory(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::distance2DTo(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::nav::TMoveEdgeSE2_TP::end_state, mrpt::format(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::getAllNodes(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::getNearestNode(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::getNextFreeNodeID(), mrpt::random::getRandomGenerator(), mrpt::nav::TPlannerResultTempl< tree_t >::goal_distance, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::goal_pose, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::ground_xy_grid_frequency, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_last_added_edge, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::highlight_path_to_node_id, mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::insertNode(), mrpt::nav::TMoveTree< NODE_TYPE_DATA, EDGE_TYPE, MAPS_IMPLEMENTATION >::insertNodeAndEdge(), INVALID_NODEID, mrpt::keep_max(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg, mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::log_msg_position, min, mrpt::nav::TPlannerResultTempl< tree_t >::move_tree, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::obstacles_points, mrpt::nav::TMoveEdgeSE2_TP::parent_id, mrpt::nav::TPlannerResultTempl< tree_t >::path_cost, mrpt::poses::CPose2D::phi(), mrpt::math::TPose2D::phi, mrpt::nav::TMoveEdgeSE2_TP::ptg_dist, mrpt::nav::TMoveEdgeSE2_TP::ptg_index, mrpt::nav::TMoveEdgeSE2_TP::ptg_K, mrpt::graphs::CDirectedTree< TYPE_EDGES >::root, mrpt::opengl::COpenGLScene::saveToFile(), mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::start_pose, mrpt::nav::TNodeSE2_TP::state, static_size, mrpt::nav::TPlannerResultTempl< tree_t >::success, mrpt::system::CTicTac::Tac(), mrpt::system::CTicTac::Tic(), mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::world_bbox_max, mrpt::nav::TPlannerInputTempl< node_pose_t, world_limits_t >::world_bbox_min, mrpt::math::wrapToPiInPlace(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::nav::PlannerTPS_VirtualBase::TRenderPlannedPathOptions::x_rand_pose, and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

◆ spaceTransformer()

void PlannerTPS_VirtualBase::spaceTransformer	(	const mrpt::maps::CSimplePointsMap &	in_obstacles,
		const mrpt::nav::CParameterizedTrajectoryGenerator *	in_PTG,
		const double	MAX_DIST,
		std::vector< double > &	out_TPObstacles
	)

protectedinherited

Definition at line 178 of file PlannerRRT_common.cpp.

References mrpt::format(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles(), MAX_DIST, and mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacle().

◆ spaceTransformerOneDirectionOnly()

void PlannerTPS_VirtualBase::spaceTransformerOneDirectionOnly	(	const int	tp_space_k_direction,
		const mrpt::maps::CSimplePointsMap &	in_obstacles,
		const mrpt::nav::CParameterizedTrajectoryGenerator *	in_PTG,
		const double	MAX_DIST,
		double &	out_TPObstacle_k
	)

protectedinherited

Definition at line 228 of file PlannerRRT_common.cpp.

References mrpt::format(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle(), MAX_DIST, and mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacleSingle().

◆ transformPointcloudWithSquareClipping()

void PlannerTPS_VirtualBase::transformPointcloudWithSquareClipping	(	const mrpt::maps::CPointsMap &	in_map,
		mrpt::maps::CPointsMap &	out_map,
		const mrpt::poses::CPose2D &	asSeenFrom,
		const double	MAX_DIST_XY
	)

staticprotectedinherited

Definition at line 144 of file PlannerRRT_common.cpp.

References mrpt::maps::CMetricMap::clear(), mrpt::poses::CPose2D::composePoint(), mrpt::maps::CPointsMap::getPointsBuffer(), mrpt::maps::CPointsMap::insertPointFast(), mrpt::maps::CPointsMap::reserve(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

Member Data Documentation

◆ end_criteria

RRTEndCriteria mrpt::nav::PlannerTPS_VirtualBase::end_criteria

inherited

Definition at line 144 of file PlannerRRT_common.h.

◆ m_initialized

bool mrpt::nav::PlannerRRT_SE2_TPS::m_initialized

protected

Definition at line 106 of file PlannerRRT_SE2_TPS.h.

◆ m_initialized_PTG

bool mrpt::nav::PlannerTPS_VirtualBase::m_initialized_PTG

protectedinherited

Definition at line 252 of file PlannerRRT_common.h.

Referenced by mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG().

◆ m_local_obs

mrpt::maps::CSimplePointsMap mrpt::nav::PlannerTPS_VirtualBase::m_local_obs

protectedinherited

Definition at line 254 of file PlannerRRT_common.h.

◆ m_PTGs

mrpt::nav::TListPTGPtr mrpt::nav::PlannerTPS_VirtualBase::m_PTGs

protectedinherited

Definition at line 253 of file PlannerRRT_common.h.

Referenced by mrpt::nav::PlannerTPS_VirtualBase::getPTGs(), mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG(), mrpt::nav::PlannerTPS_VirtualBase::internal_loadConfig_PTG(), and mrpt::nav::PlannerTPS_VirtualBase::renderMoveTree().

◆ m_timelogger

mrpt::system::CTimeLogger mrpt::nav::PlannerTPS_VirtualBase::m_timelogger

protectedinherited

Definition at line 251 of file PlannerRRT_common.h.

Referenced by mrpt::nav::PlannerTPS_VirtualBase::getProfiler(), and mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG().

◆ params

RRTAlgorithmParams mrpt::nav::PlannerTPS_VirtualBase::params

inherited

Parameters specific to this path solver algorithm.

Definition at line 146 of file PlannerRRT_common.h.

Detailed Description

Classes

Public Types

Public Member Functions

Public Attributes

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Member Typedef Documentation

◆ node_pose_t

Constructor & Destructor Documentation

◆ PlannerRRT_SE2_TPS()

Member Function Documentation

◆ getProfiler()

◆ getPTGs()

◆ initialize()

◆ internal_initialize_PTG()

◆ internal_loadConfig_PTG()

◆ loadConfig()

◆ renderMoveTree()

◆ solve()

◆ spaceTransformer()

◆ spaceTransformerOneDirectionOnly()

◆ transformPointcloudWithSquareClipping()

Member Data Documentation

◆ end_criteria

◆ m_initialized

◆ m_initialized_PTG

◆ m_local_obs

◆ m_PTGs

◆ m_timelogger

◆ params