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. For running it, the method NavigateStep must be invoked periodically.
Definition at line 75 of file CReactiveNavigationSystem.h.
#include <mrpt/reactivenav/CReactiveNavigationSystem.h>
Classes | |
class | CDynamicWindow |
For taken the dynamics of the robot into account. More... | |
struct | THolonomicMovement |
The structure used for storing a movement generated by a holonomic-method . More... | |
Public Types | |
enum | TNavigatorBehavior { beNormalNavigation = 0, beHeadDirection, beDoorCrosing1, beDoorCrosing2, beDoorCrosing3 } |
See 'navigatorBehavior'. 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. | |
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 | Error_ParadaDeEmergencia (const char *msg) |
Private Attributes | |
std::vector< float > | prevV |
std::vector< float > | prevW |
std::vector< float > | prevSelPTG |
TNavigatorBehavior | navigatorBehavior |
This defines the 'behavior' of the navigator (see posible values in TNavigatorBehavior) | |
float | m_beHeadDirection_rad |
The desired heading direction (in rads), for behaviors 'beHeadDirection' and 'beDoorCrosing2'. | |
CPoint2D | m_beAuxTarget |
Auxiliary target position, for behaviors 'beDoorCrosing1' and 'beDoorCrosing3'. | |
CPoint2D | m_bePassPoint1 |
This is the desired "path" for passing a door (in GLOBAL coordinates!) | |
CPoint2D | m_bePassPoint2 |
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 current log file stream, or NULL if not being used. | |
utils::CStream * | logFile |
The current log file stream, or NULL if not being used. | |
bool | enableConsoleOutput |
Enables / disables the console debug output. | |
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 |
float | DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 |
float | VORONOI_MINIMUM_CLEARANCE |
float | DISABLE_PERIOD_AFTER_FAIL |
float | VORONOI_PATH_DIST_FROM_DOORWAY |
float | DOORCROSSING_HEADING_ACCURACY_DEG |
float | DOORCROSSING_ROTATION_CTE_DEG |
float | DOOR_CROSSING_DIST_TO_AUX_TARGETS |
float | DOOR_CROOSING_BEH3_TIMEOUT |
float | DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE |
unsigned long | nIteration |
The iterations 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 | CerrandoHilo |
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 |
CDynamicWindow | DW |
int | m_decimateHeadingEstimate |
See 'navigatorBehavior'.
Definition at line 140 of file CReactiveNavigationSystem.h.
mrpt::reactivenav::CReactiveNavigationSystem::CReactiveNavigationSystem | ( | CReactiveInterfaceImplementation & | react_iterf_impl, |
bool | enableConsoleOutput = true , |
||
bool | enableLogFile = false |
||
) |
Constructor.
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.
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::enableLogFile | ( | bool | enable | ) |
Enables / disables the logging into a file.
void mrpt::reactivenav::CReactiveNavigationSystem::Error_ParadaDeEmergencia | ( | const char * | msg | ) | [private] |
float mrpt::reactivenav::CReactiveNavigationSystem::evaluate | ( | TNavigationParams * | params | ) | [virtual] |
Evaluate navigation hardness:
Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.
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.
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::CReactiveNavigationSystem::performNavigationStep | ( | ) | [private, virtual] |
The main method for the navigator.
Implements mrpt::reactivenav::CAbstractReactiveNavigationSystem.
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] |
Definition at line 266 of file CReactiveNavigationSystem.h.
mrpt::system::TTimeStamp mrpt::reactivenav::CReactiveNavigationSystem::badNavAlarm_lastMinDistTime [private] |
Definition at line 265 of file CReactiveNavigationSystem.h.
For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...
Definition at line 264 of file CReactiveNavigationSystem.h.
bool mrpt::reactivenav::CReactiveNavigationSystem::CerrandoHilo [private] |
Definition at line 363 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::colGridRes_x [private] |
Definition at line 235 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::colGridRes_y [private] |
Definition at line 235 of file CReactiveNavigationSystem.h.
Definition at line 272 of file CReactiveNavigationSystem.h.
Definition at line 245 of file CReactiveNavigationSystem.h.
Definition at line 241 of file CReactiveNavigationSystem.h.
Definition at line 250 of file CReactiveNavigationSystem.h.
Definition at line 249 of file CReactiveNavigationSystem.h.
Definition at line 251 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 [private] |
Definition at line 243 of file CReactiveNavigationSystem.h.
Definition at line 247 of file CReactiveNavigationSystem.h.
Definition at line 248 of file CReactiveNavigationSystem.h.
Definition at line 316 of file CReactiveNavigationSystem.h.
Enables / disables the console debug output.
Definition at line 228 of file CReactiveNavigationSystem.h.
Definition at line 307 of file CReactiveNavigationSystem.h.
CAbstractHolonomicReactiveMethod* mrpt::reactivenav::CReactiveNavigationSystem::holonomicMethod [private] |
The current log file stream, or NULL if not being used.
Definition at line 222 of file CReactiveNavigationSystem.h.
std::vector<THolonomicMovement> mrpt::reactivenav::CReactiveNavigationSystem::holonomicMovements [private] |
Definition at line 310 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::last_cmd_v [private] |
For the histeresis:
Definition at line 199 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::last_cmd_w [private] |
Definition at line 199 of file CReactiveNavigationSystem.h.
The last log.
Definition at line 195 of file CReactiveNavigationSystem.h.
The current log file stream, or NULL if not being used.
Definition at line 225 of file CReactiveNavigationSystem.h.
Auxiliary target position, for behaviors 'beDoorCrosing1' and 'beDoorCrosing3'.
Definition at line 179 of file CReactiveNavigationSystem.h.
The desired heading direction (in rads), for behaviors 'beHeadDirection' and 'beDoorCrosing2'.
Definition at line 175 of file CReactiveNavigationSystem.h.
This is the desired "path" for passing a door (in GLOBAL coordinates!)
Definition at line 183 of file CReactiveNavigationSystem.h.
Definition at line 183 of file CReactiveNavigationSystem.h.
Critical zones:
Definition at line 207 of file CReactiveNavigationSystem.h.
synch::CCriticalSection mrpt::reactivenav::CReactiveNavigationSystem::m_critZoneNavigating [private] |
Definition at line 207 of file CReactiveNavigationSystem.h.
Definition at line 317 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::maxObstaclesHeight [private] |
Definition at line 240 of file CReactiveNavigationSystem.h.
Runtime estimation of execution period of the method.
Definition at line 259 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::meanExecutionTime [private] |
Definition at line 313 of file CReactiveNavigationSystem.h.
Definition at line 314 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::minObstaclesHeight [private] |
Definition at line 240 of file CReactiveNavigationSystem.h.
Will be false until the navigation end is sent, and it is reset with each new command.
Definition at line 203 of file CReactiveNavigationSystem.h.
This defines the 'behavior' of the navigator (see posible values in TNavigatorBehavior)
Definition at line 171 of file CReactiveNavigationSystem.h.
unsigned long mrpt::reactivenav::CReactiveNavigationSystem::nIteration [private] |
The iterations count.
Definition at line 255 of file CReactiveNavigationSystem.h.
Definition at line 315 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::prevSelPTG [private] |
Definition at line 167 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::prevV [private] |
Definition at line 167 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::prevW [private] |
Definition at line 167 of file CReactiveNavigationSystem.h.
std::vector<CParameterizedTrajectoryGenerator*> mrpt::reactivenav::CReactiveNavigationSystem::PTGs [private] |
The set of transformations to be used:
Definition at line 322 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::refDistance [private] |
Definition at line 234 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::robotMax_V_mps [private] |
Definition at line 236 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::robotMax_W_degps [private] |
Definition at line 237 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::ROBOTMODEL_DELAY [private] |
Definition at line 238 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::ROBOTMODEL_TAU [private] |
Definition at line 238 of file CReactiveNavigationSystem.h.
std::string mrpt::reactivenav::CReactiveNavigationSystem::robotName [private] |
Definition at line 233 of file CReactiveNavigationSystem.h.
The robot 2D shape model.
Definition at line 271 of file CReactiveNavigationSystem.h.
Definition at line 307 of file CReactiveNavigationSystem.h.
Definition at line 230 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::times_HoloNav [private] |
Definition at line 311 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::times_TP_transformations [private] |
Definition at line 311 of file CReactiveNavigationSystem.h.
Definition at line 307 of file CReactiveNavigationSystem.h.
std::vector<vector_double> mrpt::reactivenav::CReactiveNavigationSystem::TP_Obstacles [private] |
Definition at line 308 of file CReactiveNavigationSystem.h.
std::vector<poses::CPoint2D,Eigen::aligned_allocator<poses::CPoint2D> > mrpt::reactivenav::CReactiveNavigationSystem::TP_Targets [private] |
Definition at line 309 of file CReactiveNavigationSystem.h.
std::vector<bool> mrpt::reactivenav::CReactiveNavigationSystem::valid_TP [private] |
Definition at line 312 of file CReactiveNavigationSystem.h.
Definition at line 244 of file CReactiveNavigationSystem.h.
Definition at line 246 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::weights [private] |
Definition at line 239 of file CReactiveNavigationSystem.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |