Detailed Description

This class is a multi-threaded mobile robot navigator, implementing an (anytime) PTG-based Rapidly-exploring Random Tree (PRRT) search algorithm.

Usage:

Create an instance of the CPRRTNavigator class (an object on the heap, i.e. no 'new', is preferred, but just for the convenience of the user).
Set all the parameters in CPRRTNavigator::params
Call CPRRTNavigator::initialize. If all the params are OK, true is returned and you can go on.
Start a navigation by calling CPRRTNavigator::navigate.
From your application, you must update all the sensory data (from the real robot or a simulator) on a timely-fashion. The navigator will stop the robot if the last sensory data is too old.

Note that all the public methods are thread-safe.

About the algorithm:

Changes history

05/JUL/2009: Creation (JLBC). This is an evolution from leassons learnt from the pure reactive navigator based on PTGs.

Definition at line 52 of file CPRRTNavigator.h.

#include <mrpt/reactivenav/CPRRTNavigator.h>

Classes
class	TOptions

struct	TPathData
	Each data point in m_planned_path. More...

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

virtual	~CPRRTNavigator ()
	Destructor. More...

Navigation control methods
bool	initialize ()
	Initialize the navigator using the parameters in "params"; read the usage instructions in CPRRTNavigator. More...

void	navigate (const mrpt::math::TPose2D &target_pose)
	Starts a navigation to a 2D pose (including a desired heading at the target). More...

void	navigate (const mrpt::math::TPoint2D &target_point)
	Starts a navigation to a given 2D point (any heading at the target). More...

void	cancel ()
	Cancel current navegacion. More...

void	resume ()
	Continues with suspended navigation. More...

void	suspend ()
	Suspend current navegation. More...

Debug and logging
void	setPathToFollow (const TPlannedPath &path)
	Manually sets the short-time path to be followed by the robot (use 'navigate' instead, this method is for debugging mainly) More...

void	getCurrentPlannedPath (TPlannedPath &path) const
	Returns the current planned path the robot is following. More...

Sensory data methods: call them to update the navigator knowledge on the outside world.
void	processNewLocalization (const TPose2D &new_robot_pose, const CMatrixDouble33 &new_robot_cov, TTimeStamp timestamp)
	Updates the navigator with a low or high-rate estimate from a localization (or SLAM) algorithm running somewhere else. More...

void	processNewOdometryInfo (const TPose2D &newOdoPose, TTimeStamp timestamp, bool hasVelocities=false, float v=0, float w=0)
	Updates the navigator with high-rate odometry from the mobile base. More...

void	processNewObstaclesData (const mrpt::slam::CPointsMap *obstacles, TTimeStamp timestamp)
	Must be called in a timely fashion to let the navigator know about the obstacles in the environment. More...

Virtual methods to be implemented by the user.
virtual bool	onMotionCommand (float v, float w)=0
	This is the most important method the user must provide: to send an instantaneous velocity command to the robot. More...

virtual void	onNavigationStart ()
	Re-implement this method if you want to know when a new navigation has started. More...

virtual void	onNavigationEnd (bool targetReachedOK)
	Re-implement this method if you want to know when and why a navigation has ended. More...

virtual void	onApproachingTarget ()
	Re-implement this method if you want to know when the robot is approaching the target: this event is raised before onNavigationEnd, when the robot is closer than a certain distance to the target. More...

Public Attributes
mrpt::poses::CRobot2DPoseEstimator	m_robotStateFilter
	Object maintained by the robot-tracking thread (All methods are thread-safe). More...

Static Public Attributes
static const double	INVALID_HEADING
	Used to refer to undefined or "never mind" headings values. More...

Private Member Functions
void	thread_planner ()
	Thread function. More...

void	thread_test_collision ()
	Thread function. More...

void	thread_path_tracking ()
	Thread function. More...

Private Attributes
mrpt::system::TThreadHandle	m_thr_planner
	Thread handle. More...

mrpt::system::TThreadHandle	m_thr_testcol
	Thread handle. More...

mrpt::system::TThreadHandle	m_thr_pathtrack
	Thread handle. More...

bool	m_initialized
	The object is ready to navigate. Users must call "initialize" first. More...

bool	m_closingThreads
	set to true at destructor. More...

TPose2D	m_target_pose
	Heading may be INVALID_HEADING to indicate "don't mind". More...

TTimeStamp	m_target_pose_time

CCriticalSection	m_target_pose_cs

std::vector< float >	m_last_obstacles_x

std::vector< float >	m_last_obstacles_y

TTimeStamp	m_last_obstacles_time

CCriticalSection	m_last_obstacles_cs

CCriticalSection	m_planned_path_cs

TTimeStamp	m_planned_path_time
	The last modification time. INVALID_TIMESTAMP means there is no path. More...

TPlannedPath	m_planned_path

TListPTGs	m_PTGs
	The list of PTGs used by the anytime planner to explore the free-space. More...

CCriticalSection	m_PTGs_cs

Navigator data structures
typedef std::list< TPathData >	TPlannedPath
	An ordered list of path poses. More...

TOptions	params
	Change here all the parameters of the navigator. More...

Member Typedef Documentation

typedef std::list<TPathData> mrpt::reactivenav::CPRRTNavigator::TPlannedPath

An ordered list of path poses.

Definition at line 114 of file CPRRTNavigator.h.

Constructor & Destructor Documentation

mrpt::reactivenav::CPRRTNavigator::CPRRTNavigator ( )

Constructor.

virtual mrpt::reactivenav::CPRRTNavigator::~CPRRTNavigator ( )

virtual

Destructor.

Member Function Documentation

void mrpt::reactivenav::CPRRTNavigator::cancel ( )

Cancel current navegacion.

Note: CPRRTNavigator::initialize must be called first.

void mrpt::reactivenav::CPRRTNavigator::getCurrentPlannedPath ( TPlannedPath & path ) const

Returns the current planned path the robot is following.

bool mrpt::reactivenav::CPRRTNavigator::initialize ( )

Initialize the navigator using the parameters in "params"; read the usage instructions in CPRRTNavigator.

This method should be called only once at the beginning of the robot operation.

Returns: true on sucess, false on any error preparing the navigator (and no navigation command will be processed until that's fixed).

void mrpt::reactivenav::CPRRTNavigator::navigate ( const mrpt::math::TPose2D & target_pose )

Starts a navigation to a 2D pose (including a desired heading at the target).

Note: CPRRTNavigator::initialize must be called first.

void mrpt::reactivenav::CPRRTNavigator::navigate ( const mrpt::math::TPoint2D & target_point )

Starts a navigation to a given 2D point (any heading at the target).

Note: CPRRTNavigator::initialize must be called first.

virtual void mrpt::reactivenav::CPRRTNavigator::onApproachingTarget ( )

inlinevirtual

Re-implement this method if you want to know when the robot is approaching the target: this event is raised before onNavigationEnd, when the robot is closer than a certain distance to the target.

Definition at line 228 of file CPRRTNavigator.h.

virtual bool mrpt::reactivenav::CPRRTNavigator::onMotionCommand	(	float	v,
		float	w
	)

pure virtual

This is the most important method the user must provide: to send an instantaneous velocity command to the robot.

Parameters

v	Desired linear speed, in meters/second.
w	Desired angular speed, in rads/second.

Returns: false on any error. In that case, the navigator will immediately stop the navigation and announce the error.

virtual void mrpt::reactivenav::CPRRTNavigator::onNavigationEnd ( bool targetReachedOK )

inlinevirtual

Re-implement this method if you want to know when and why a navigation has ended.

Parameters

targetReachedOK Will be false if the navigation failed.

Definition at line 221 of file CPRRTNavigator.h.

References MRPT_UNUSED_PARAM.

virtual void mrpt::reactivenav::CPRRTNavigator::onNavigationStart ( )

inlinevirtual

Re-implement this method if you want to know when a new navigation has started.

Definition at line 216 of file CPRRTNavigator.h.

void mrpt::reactivenav::CPRRTNavigator::processNewLocalization	(	const TPose2D &	new_robot_pose,
		const CMatrixDouble33 &	new_robot_cov,
		TTimeStamp	timestamp
	)

Updates the navigator with a low or high-rate estimate from a localization (or SLAM) algorithm running somewhere else.

Parameters

new_robot_pose	The (global) coordinates of the mean robot pose as estimated by some external module.
new_robot_cov	The 3x3 covariance matrix of that estimate.
timestamp	The associated timestamp of the data.

void mrpt::reactivenav::CPRRTNavigator::processNewObstaclesData	(	const mrpt::slam::CPointsMap *	obstacles,
		TTimeStamp	timestamp
	)

Must be called in a timely fashion to let the navigator know about the obstacles in the environment.

Parameters

obstacles
timestamp	The associated timestamp of the sensed points.

void mrpt::reactivenav::CPRRTNavigator::processNewOdometryInfo	(	const TPose2D &	newOdoPose,
		TTimeStamp	timestamp,
		bool	hasVelocities = `false`,
		float	v = `0`,
		float	w = `0`
	)

Updates the navigator with high-rate odometry from the mobile base.

The odometry poses are dealed internally as increments only, so there is no problem is arbitrary mismatches between localization (from a particle filter or SLAM) and the odometry.

Parameters

newOdoPose	The global, cummulative odometry as computed by the robot.
timestamp	The associated timestamp of the measurement.
hasVelocities	If false, the next arguments are ignored.
v	Measured linear speed, in m/s.
w	Measured angular speed, in rad/s.

void mrpt::reactivenav::CPRRTNavigator::resume ( )

Continues with suspended navigation.

See also: suspend

Note: CPRRTNavigator::initialize must be called first.

void mrpt::reactivenav::CPRRTNavigator::setPathToFollow ( const TPlannedPath & path )

Manually sets the short-time path to be followed by the robot (use 'navigate' instead, this method is for debugging mainly)

void mrpt::reactivenav::CPRRTNavigator::suspend ( )

Suspend current navegation.

See also: resume

Note: CPRRTNavigator::initialize must be called first.

void mrpt::reactivenav::CPRRTNavigator::thread_path_tracking ( )

private

Thread function.

void mrpt::reactivenav::CPRRTNavigator::thread_planner ( )

private

Thread function.

void mrpt::reactivenav::CPRRTNavigator::thread_test_collision ( )

private

Thread function.

Member Data Documentation

const double mrpt::reactivenav::CPRRTNavigator::INVALID_HEADING

static

Used to refer to undefined or "never mind" headings values.

Definition at line 232 of file CPRRTNavigator.h.

bool mrpt::reactivenav::CPRRTNavigator::m_closingThreads

private

set to true at destructor.

Definition at line 249 of file CPRRTNavigator.h.

bool mrpt::reactivenav::CPRRTNavigator::m_initialized

private

The object is ready to navigate. Users must call "initialize" first.

Definition at line 248 of file CPRRTNavigator.h.

CCriticalSection mrpt::reactivenav::CPRRTNavigator::m_last_obstacles_cs

private

Definition at line 261 of file CPRRTNavigator.h.

TTimeStamp mrpt::reactivenav::CPRRTNavigator::m_last_obstacles_time

private

Definition at line 260 of file CPRRTNavigator.h.

std::vector<float> mrpt::reactivenav::CPRRTNavigator::m_last_obstacles_x

private

Definition at line 259 of file CPRRTNavigator.h.

std::vector<float> mrpt::reactivenav::CPRRTNavigator::m_last_obstacles_y

private

Definition at line 259 of file CPRRTNavigator.h.

TPlannedPath mrpt::reactivenav::CPRRTNavigator::m_planned_path

private

Definition at line 266 of file CPRRTNavigator.h.

CCriticalSection mrpt::reactivenav::CPRRTNavigator::m_planned_path_cs

private

Definition at line 264 of file CPRRTNavigator.h.

TTimeStamp mrpt::reactivenav::CPRRTNavigator::m_planned_path_time

private

The last modification time. INVALID_TIMESTAMP means there is no path.

Definition at line 265 of file CPRRTNavigator.h.

TListPTGs mrpt::reactivenav::CPRRTNavigator::m_PTGs

private

The list of PTGs used by the anytime planner to explore the free-space.

Definition at line 269 of file CPRRTNavigator.h.

CCriticalSection mrpt::reactivenav::CPRRTNavigator::m_PTGs_cs

private

Definition at line 270 of file CPRRTNavigator.h.

mrpt::poses::CRobot2DPoseEstimator mrpt::reactivenav::CPRRTNavigator::m_robotStateFilter

Object maintained by the robot-tracking thread (All methods are thread-safe).

Definition at line 274 of file CPRRTNavigator.h.

TPose2D mrpt::reactivenav::CPRRTNavigator::m_target_pose

private

Heading may be INVALID_HEADING to indicate "don't mind".

Definition at line 251 of file CPRRTNavigator.h.

CCriticalSection mrpt::reactivenav::CPRRTNavigator::m_target_pose_cs

private

Definition at line 253 of file CPRRTNavigator.h.

TTimeStamp mrpt::reactivenav::CPRRTNavigator::m_target_pose_time

private

Definition at line 252 of file CPRRTNavigator.h.

mrpt::system::TThreadHandle mrpt::reactivenav::CPRRTNavigator::m_thr_pathtrack

private

Thread handle.

Definition at line 239 of file CPRRTNavigator.h.

mrpt::system::TThreadHandle mrpt::reactivenav::CPRRTNavigator::m_thr_planner

private

Thread handle.

Definition at line 237 of file CPRRTNavigator.h.

mrpt::system::TThreadHandle mrpt::reactivenav::CPRRTNavigator::m_thr_testcol

private

Thread handle.

Definition at line 238 of file CPRRTNavigator.h.

TOptions mrpt::reactivenav::CPRRTNavigator::params

Change here all the parameters of the navigator.

Definition at line 160 of file CPRRTNavigator.h.

Detailed Description

Classes

Public Member Functions

Public Attributes

Static Public Attributes

Private Member Functions

Private Attributes

Navigator data structures

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation