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

#include <mrpt/reactivenav/CReactiveNavigationSystem.h>

Inheritance diagram for mrpt::reactivenav::CReactiveNavigationSystem:

[legend]

List of all members.

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 [inline, inherited]

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 ( ) [private, virtual]

The main method for the navigator.

Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.

static void mrpt::utils::CDebugOutputCapable::printf_debug	(	const char *	frmt,
			...
	)		`[static, inherited]`

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

Referenced by mrpt::math::CLevenbergMarquardtTempl::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]`