Detailed Description

Implements a reactive navigation system based on TP-Space, with an arbitrary holonomic reactive method running on it, and any desired number of PTG for transforming the navigation space.

Both, the holonomic method and the PTGs can be customized by the apropriate user derived classes.

How to use:

A class with callbacks must be defined by the user and provided to the constructor.
loadConfigFile() must be called to set up the bunch of parameters from a config file (could be a memory-based virtual config file).
navigationStep() must be called periodically in order to effectively run the navigation. This method will internally call the callbacks to gather sensor data and robot positioning data.

17/JUN/2004: First design.
16/SEP/2004: Totally redesigned, according to document "MultiParametric Based Space Transformation for Reactive Navigation"
29/SEP/2005: Totally rewritten again, for integration into MRPT library and according to the ICRA paper.
17/OCT/2007: Whole code updated to accomodate to MRPT 0.5 and make it portable to Linux.

See Also: CAbstractReactiveNavigationSystem, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod

Definition at line 76 of file CReactiveNavigationSystem.h.

#include <mrpt/reactivenav/CReactiveNavigationSystem.h>

Inheritance diagram for mrpt::reactivenav::CReactiveNavigationSystem:

[legend]

Classes
struct	THolonomicMovement
	The structure used for storing a movement generated by a holonomic-method . More...

Public Types
enum	TState { IDLE =0, NAVIGATING, SUSPENDED, NAV_ERROR }
	The different states for the navigation system. More...

Public Member Functions
	CReactiveNavigationSystem (CReactiveInterfaceImplementation &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false)
	Constructor.

virtual	~CReactiveNavigationSystem ()
	Destructor.

void	loadConfigFile (const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni)
	Reload the configuration from a file.

void	initialize ()
	Must be called for loading collision grids, or the first navigation command may last a long time to be executed.

float	evaluate (TNavigationParams *params)
	Evaluate navigation hardness:

void	navigate (TNavigationParams *params)
	Start navigation:

void	setParams (TNavigationParams *params)
	Change current navigation params:

void	setHolonomicMethod (THolonomicMethod method, const char *config_INIfile="./CONFIG_ReactiveNavigator.ini")
	Selects which one from the set of available holonomic methods will be used into transformed TP-Space, and sets its configuration from a configuration file.

void	changeRobotShape (math::CPolygon &shape)
	Change the robot shape, which is taken into account for collision grid building.

void	getLastLogRecord (CLogFileRecord &o)
	Provides a copy of the last log record with information about execution.

void	enableLogFile (bool enable)
	Enables / disables the logging into a file.

void	cancel ()
	Cancel current navegacion.

void	resume ()
	Continues with suspended navigation.

void	navigationStep ()
	This method must be called periodically in order to effectively run the navigation.

virtual void	suspend ()
	Suspend current navegation.

TState	getCurrentState () const
	Returns the current navigator state.

Static Public Member Functions
static void	printf_debug (const char *frmt,...)
	Sends a formated text to "debugOut" if not NULL, or to cout otherwise.

Protected Attributes
TState	m_navigationState
	Current internal state of navigator:

TNavigationParams	m_navigationParams
	Current navigation parameters:

CReactiveInterfaceImplementation &	m_robot
	The navigator-robot interface.

Private Member Functions
void	performNavigationStep ()
	The main method for the navigator.

void	STEP1_CollisionGridsBuilder ()

bool	STEP2_Sense (mrpt::slam::CSimplePointsMap &out_obstacles)

void	STEP3_SpaceTransformer (poses::CPointsMap &in_obstacles, CParameterizedTrajectoryGenerator *in_PTG, vector_double &out_TPObstacles)

void	STEP4_HolonomicMethod (vector_double &in_Obstacles, poses::CPoint2D &in_Target, float in_maxRobotSpeed, THolonomicMovement &out_selectedMovement, CHolonomicLogFileRecordPtr &in_HLFR)

void	STEP5_Evaluator (THolonomicMovement &in_holonomicMovement, vector_double &in_TPObstacles, poses::CPoint2D &WS_Target, poses::CPoint2D &TP_Target, bool wasSelectedInLast, CLogFileRecord::TInfoPerPTG &log)

void	STEP6_Selector (std::vector< THolonomicMovement > &in_holonomicMovements, THolonomicMovement &out_selectedHolonomicMovement, int &out_nSelectedPTG)

void	STEP7_NonHolonomicMovement (THolonomicMovement &in_movement, float &out_v, float &out_w)

void	doEmergencyStop (const char *msg)
	Stops the robot and set navigation state to error.

Private Attributes
CLogFileRecord	lastLogRecord
	The last log.

float	last_cmd_v
	For the histeresis:

float	last_cmd_w

bool	navigationEndEventSent
	Will be false until the navigation end is sent, and it is reset with each new command.

synch::CCriticalSection	m_critZoneLastLog
	Critical zones:

synch::CCriticalSection	m_critZoneNavigating

CAbstractHolonomicReactiveMethod *	holonomicMethod
	The holonomic navigation algorithm.

mrpt::utils::CStream *	logFile
	The current log file stream, or NULL if not being used.

bool	m_enableConsoleOutput
	Enables / disables the console debug output.

bool	m_init_done
	Whether loadConfigFile() has been called or not.

CTicTac	timerForExecutionPeriod

std::string	robotName

float	refDistance

float	colGridRes_x

float	colGridRes_y

float	robotMax_V_mps

float	robotMax_W_degps

float	ROBOTMODEL_TAU

float	ROBOTMODEL_DELAY

std::vector< float >	weights

float	minObstaclesHeight

float	maxObstaclesHeight

float	DIST_TO_TARGET_FOR_SENDING_EVENT

unsigned long	nIteration
	The iteration count.

float	meanExecutionPeriod
	Runtime estimation of execution period of the method.

float	badNavAlarm_minDistTarget
	For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...

mrpt::system::TTimeStamp	badNavAlarm_lastMinDistTime

float	badNavAlarm_AlarmTimeout

math::CPolygon	robotShape
	The robot 2D shape model.

bool	collisionGridsMustBeUpdated

std::vector < CParameterizedTrajectoryGenerator * >	PTGs
	The set of transformations to be used:

bool	m_closing_navigator

Variables for CReactiveNavigationSystem::performNavigationStep
mrpt::utils::CTicTac	totalExecutionTime

mrpt::utils::CTicTac	executionTime

mrpt::utils::CTicTac	tictac

std::vector< vector_double >	TP_Obstacles

std::vector< poses::CPoint2D, Eigen::aligned_allocator < poses::CPoint2D > >	TP_Targets

std::vector< THolonomicMovement >	holonomicMovements

std::vector< float >	times_TP_transformations

std::vector< float >	times_HoloNav

std::vector< bool >	valid_TP

float	meanExecutionTime

float	meanTotalExecutionTime

int	nLastSelectedPTG

int	m_decimateHeadingEstimate

Member Enumeration Documentation

enum mrpt::reactivenav::CAbstractReactiveNavigationSystem::TState

inherited

The different states for the navigation system.

Enumerator
IDLE
NAVIGATING
SUSPENDED
NAV_ERROR

Definition at line 190 of file CAbstractReactiveNavigationSystem.h.

Constructor & Destructor Documentation

mrpt::reactivenav::CReactiveNavigationSystem::CReactiveNavigationSystem	(	CReactiveInterfaceImplementation &	react_iterf_impl,
		bool	enableConsoleOutput = `true`,
		bool	enableLogFile = `false`
	)

Constructor.

Parameters

configINIFile	The file to load the configuration from. See loadConfigFile
robotConfigFile	The file to load the robot specific configuration from.
rmc	A set of wrappers that must be filled in.
sensors	A set of wrappers that must be filled in.
dbg	A set of wrappers that must be filled in.
evnts	A set of wrappers that must be filled in.
enableConsoleOutput	Set to false if console output is not desired.
enableLogFile	Set to true to enable logging to file.

virtual mrpt::reactivenav::CReactiveNavigationSystem::~CReactiveNavigationSystem ( )

virtual

Destructor.

Member Function Documentation

void mrpt::reactivenav::CAbstractReactiveNavigationSystem::cancel ( )

inherited

Cancel current navegacion.

void mrpt::reactivenav::CReactiveNavigationSystem::changeRobotShape ( math::CPolygon & shape )

Change the robot shape, which is taken into account for collision grid building.

void mrpt::reactivenav::CReactiveNavigationSystem::doEmergencyStop ( const char * msg )

private

Stops the robot and set navigation state to error.

void mrpt::reactivenav::CReactiveNavigationSystem::enableLogFile ( bool enable )

Enables / disables the logging into a file.

float mrpt::reactivenav::CReactiveNavigationSystem::evaluate ( TNavigationParams * params )

virtual

Evaluate navigation hardness:

Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.

TState mrpt::reactivenav::CAbstractReactiveNavigationSystem::getCurrentState ( ) const

inlineinherited

Returns the current navigator state.

Definition at line 200 of file CAbstractReactiveNavigationSystem.h.

void mrpt::reactivenav::CReactiveNavigationSystem::getLastLogRecord ( CLogFileRecord & o )

Provides a copy of the last log record with information about execution.

On any unexpected error "*o" will be NULL.

Parameters

o	An object where the log will be stored into.

void mrpt::reactivenav::CReactiveNavigationSystem::initialize ( )

Must be called for loading collision grids, or the first navigation command may last a long time to be executed.

void mrpt::reactivenav::CReactiveNavigationSystem::loadConfigFile	(	const mrpt::utils::CConfigFileBase &	ini,
		const mrpt::utils::CConfigFileBase &	robotIni
	)

Reload the configuration from a file.

void mrpt::reactivenav::CReactiveNavigationSystem::navigate ( TNavigationParams * params )

virtual

Start navigation:

Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.

void mrpt::reactivenav::CAbstractReactiveNavigationSystem::navigationStep ( )

inherited

This method must be called periodically in order to effectively run the navigation.

void mrpt::reactivenav::CReactiveNavigationSystem::performNavigationStep ( )

privatevirtual

The main method for the navigator.

Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.

static void mrpt::utils::CDebugOutputCapable::printf_debug	(	const char *	frmt,
			...
	)

staticinherited

Sends a formated text to "debugOut" if not NULL, or to cout otherwise.

Referenced by mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute().

void mrpt::reactivenav::CAbstractReactiveNavigationSystem::resume ( )

inherited

Continues with suspended navigation.

See Also: suspend

void mrpt::reactivenav::CReactiveNavigationSystem::setHolonomicMethod	(	THolonomicMethod	method,
		const char *	config_INIfile = `"./CONFIG_ReactiveNavigator.ini"`
	)

Selects which one from the set of available holonomic methods will be used into transformed TP-Space, and sets its configuration from a configuration file.

void mrpt::reactivenav::CReactiveNavigationSystem::setParams ( TNavigationParams * params )

virtual

Change current navigation params:

Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.

void mrpt::reactivenav::CReactiveNavigationSystem::STEP1_CollisionGridsBuilder ( )

private

bool mrpt::reactivenav::CReactiveNavigationSystem::STEP2_Sense ( mrpt::slam::CSimplePointsMap & out_obstacles )

private

void mrpt::reactivenav::CReactiveNavigationSystem::STEP3_SpaceTransformer	(	poses::CPointsMap &	in_obstacles,
		CParameterizedTrajectoryGenerator *	in_PTG,
		vector_double &	out_TPObstacles
	)

private

void mrpt::reactivenav::CReactiveNavigationSystem::STEP4_HolonomicMethod	(	vector_double &	in_Obstacles,
		poses::CPoint2D &	in_Target,
		float	in_maxRobotSpeed,
		THolonomicMovement &	out_selectedMovement,
		CHolonomicLogFileRecordPtr &	in_HLFR
	)

private

void mrpt::reactivenav::CReactiveNavigationSystem::STEP5_Evaluator	(	THolonomicMovement &	in_holonomicMovement,
		vector_double &	in_TPObstacles,
		poses::CPoint2D &	WS_Target,
		poses::CPoint2D &	TP_Target,
		bool	wasSelectedInLast,
		CLogFileRecord::TInfoPerPTG &	log
	)

private

void mrpt::reactivenav::CReactiveNavigationSystem::STEP6_Selector	(	std::vector< THolonomicMovement > &	in_holonomicMovements,
		THolonomicMovement &	out_selectedHolonomicMovement,
		int &	out_nSelectedPTG
	)

private

void mrpt::reactivenav::CReactiveNavigationSystem::STEP7_NonHolonomicMovement	(	THolonomicMovement &	in_movement,
		float &	out_v,
		float &	out_w
	)

private

virtual void mrpt::reactivenav::CAbstractReactiveNavigationSystem::suspend ( )

virtualinherited

Suspend current navegation.

See Also: resume

Member Data Documentation

float mrpt::reactivenav::CReactiveNavigationSystem::badNavAlarm_AlarmTimeout

private

Definition at line 207 of file CReactiveNavigationSystem.h.

mrpt::system::TTimeStamp mrpt::reactivenav::CReactiveNavigationSystem::badNavAlarm_lastMinDistTime

private

Definition at line 206 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::badNavAlarm_minDistTarget

private

For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...

Definition at line 205 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::colGridRes_x

private

Definition at line 190 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::colGridRes_y

private

Definition at line 190 of file CReactiveNavigationSystem.h.

bool mrpt::reactivenav::CReactiveNavigationSystem::collisionGridsMustBeUpdated

private

Definition at line 213 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::DIST_TO_TARGET_FOR_SENDING_EVENT

private

Definition at line 196 of file CReactiveNavigationSystem.h.

mrpt::utils::CTicTac mrpt::reactivenav::CReactiveNavigationSystem::executionTime

private

Definition at line 218 of file CReactiveNavigationSystem.h.

CAbstractHolonomicReactiveMethod* mrpt::reactivenav::CReactiveNavigationSystem::holonomicMethod

private

The holonomic navigation algorithm.

Definition at line 178 of file CReactiveNavigationSystem.h.

std::vector<THolonomicMovement> mrpt::reactivenav::CReactiveNavigationSystem::holonomicMovements

private

Definition at line 221 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::last_cmd_v

private

For the histeresis:

Definition at line 159 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::last_cmd_w

private

Definition at line 159 of file CReactiveNavigationSystem.h.

CLogFileRecord mrpt::reactivenav::CReactiveNavigationSystem::lastLogRecord

private

The last log.

Definition at line 156 of file CReactiveNavigationSystem.h.

mrpt::utils::CStream* mrpt::reactivenav::CReactiveNavigationSystem::logFile

private

The current log file stream, or NULL if not being used.

Definition at line 179 of file CReactiveNavigationSystem.h.

bool mrpt::reactivenav::CReactiveNavigationSystem::m_closing_navigator

private

Definition at line 273 of file CReactiveNavigationSystem.h.

synch::CCriticalSection mrpt::reactivenav::CReactiveNavigationSystem::m_critZoneLastLog

private

Critical zones:

Definition at line 166 of file CReactiveNavigationSystem.h.

synch::CCriticalSection mrpt::reactivenav::CReactiveNavigationSystem::m_critZoneNavigating

private

Definition at line 166 of file CReactiveNavigationSystem.h.

int mrpt::reactivenav::CReactiveNavigationSystem::m_decimateHeadingEstimate

private

Definition at line 227 of file CReactiveNavigationSystem.h.

bool mrpt::reactivenav::CReactiveNavigationSystem::m_enableConsoleOutput

private

Enables / disables the console debug output.

Definition at line 181 of file CReactiveNavigationSystem.h.

bool mrpt::reactivenav::CReactiveNavigationSystem::m_init_done

private

Whether loadConfigFile() has been called or not.

Definition at line 183 of file CReactiveNavigationSystem.h.

TNavigationParams mrpt::reactivenav::CAbstractReactiveNavigationSystem::m_navigationParams

protectedinherited

Current navigation parameters:

Definition at line 218 of file CAbstractReactiveNavigationSystem.h.

TState mrpt::reactivenav::CAbstractReactiveNavigationSystem::m_navigationState

protectedinherited

Current internal state of navigator:

Definition at line 214 of file CAbstractReactiveNavigationSystem.h.

CReactiveInterfaceImplementation& mrpt::reactivenav::CAbstractReactiveNavigationSystem::m_robot

protectedinherited

The navigator-robot interface.

Definition at line 221 of file CAbstractReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::maxObstaclesHeight

private

Definition at line 195 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::meanExecutionPeriod

private

Runtime estimation of execution period of the method.

Definition at line 200 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::meanExecutionTime

private

Definition at line 224 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::meanTotalExecutionTime

private

Definition at line 225 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::minObstaclesHeight

private

Definition at line 195 of file CReactiveNavigationSystem.h.

bool mrpt::reactivenav::CReactiveNavigationSystem::navigationEndEventSent

private

Will be false until the navigation end is sent, and it is reset with each new command.

Definition at line 163 of file CReactiveNavigationSystem.h.

unsigned long mrpt::reactivenav::CReactiveNavigationSystem::nIteration

private

The iteration count.

Definition at line 199 of file CReactiveNavigationSystem.h.

int mrpt::reactivenav::CReactiveNavigationSystem::nLastSelectedPTG

private

Definition at line 226 of file CReactiveNavigationSystem.h.

std::vector<CParameterizedTrajectoryGenerator*> mrpt::reactivenav::CReactiveNavigationSystem::PTGs

private

The set of transformations to be used:

Definition at line 232 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::refDistance

private

Definition at line 189 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::robotMax_V_mps

private

Definition at line 191 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::robotMax_W_degps

private

Definition at line 192 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::ROBOTMODEL_DELAY

private

Definition at line 193 of file CReactiveNavigationSystem.h.

float mrpt::reactivenav::CReactiveNavigationSystem::ROBOTMODEL_TAU

private

Definition at line 193 of file CReactiveNavigationSystem.h.

std::string mrpt::reactivenav::CReactiveNavigationSystem::robotName

private

Definition at line 188 of file CReactiveNavigationSystem.h.

math::CPolygon mrpt::reactivenav::CReactiveNavigationSystem::robotShape

private

The robot 2D shape model.

Definition at line 212 of file CReactiveNavigationSystem.h.

mrpt::utils::CTicTac mrpt::reactivenav::CReactiveNavigationSystem::tictac

private

Definition at line 218 of file CReactiveNavigationSystem.h.

CTicTac mrpt::reactivenav::CReactiveNavigationSystem::timerForExecutionPeriod

private

Definition at line 185 of file CReactiveNavigationSystem.h.

std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::times_HoloNav

private

Definition at line 222 of file CReactiveNavigationSystem.h.

std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::times_TP_transformations

private

Definition at line 222 of file CReactiveNavigationSystem.h.

mrpt::utils::CTicTac mrpt::reactivenav::CReactiveNavigationSystem::totalExecutionTime

private

Definition at line 218 of file CReactiveNavigationSystem.h.

std::vector<vector_double> mrpt::reactivenav::CReactiveNavigationSystem::TP_Obstacles

private

Definition at line 219 of file CReactiveNavigationSystem.h.

std::vector<poses::CPoint2D,Eigen::aligned_allocator<poses::CPoint2D> > mrpt::reactivenav::CReactiveNavigationSystem::TP_Targets

private

Definition at line 220 of file CReactiveNavigationSystem.h.

std::vector<bool> mrpt::reactivenav::CReactiveNavigationSystem::valid_TP

private

Definition at line 223 of file CReactiveNavigationSystem.h.

std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::weights

private

Definition at line 194 of file CReactiveNavigationSystem.h.

Detailed Description

Classes

Public Types

Public Member Functions

Static Public Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation