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 65 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 130 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 256 of file CReactiveNavigationSystem.h.
mrpt::system::TTimeStamp mrpt::reactivenav::CReactiveNavigationSystem::badNavAlarm_lastMinDistTime [private] |
Definition at line 255 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 254 of file CReactiveNavigationSystem.h.
bool mrpt::reactivenav::CReactiveNavigationSystem::CerrandoHilo [private] |
Definition at line 353 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::colGridRes_x [private] |
Definition at line 225 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::colGridRes_y [private] |
Definition at line 225 of file CReactiveNavigationSystem.h.
Definition at line 262 of file CReactiveNavigationSystem.h.
Definition at line 235 of file CReactiveNavigationSystem.h.
Definition at line 231 of file CReactiveNavigationSystem.h.
Definition at line 240 of file CReactiveNavigationSystem.h.
Definition at line 239 of file CReactiveNavigationSystem.h.
Definition at line 241 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 [private] |
Definition at line 233 of file CReactiveNavigationSystem.h.
Definition at line 237 of file CReactiveNavigationSystem.h.
Definition at line 238 of file CReactiveNavigationSystem.h.
Definition at line 306 of file CReactiveNavigationSystem.h.
Enables / disables the console debug output.
Definition at line 218 of file CReactiveNavigationSystem.h.
Definition at line 297 of file CReactiveNavigationSystem.h.
CAbstractHolonomicReactiveMethod* mrpt::reactivenav::CReactiveNavigationSystem::holonomicMethod [private] |
The current log file stream, or NULL if not being used.
Definition at line 212 of file CReactiveNavigationSystem.h.
std::vector<THolonomicMovement> mrpt::reactivenav::CReactiveNavigationSystem::holonomicMovements [private] |
Definition at line 300 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::last_cmd_v [private] |
For the histeresis:
Definition at line 189 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::last_cmd_w [private] |
Definition at line 189 of file CReactiveNavigationSystem.h.
The last log.
Definition at line 185 of file CReactiveNavigationSystem.h.
The current log file stream, or NULL if not being used.
Definition at line 215 of file CReactiveNavigationSystem.h.
Auxiliary target position, for behaviors 'beDoorCrosing1' and 'beDoorCrosing3'.
Definition at line 169 of file CReactiveNavigationSystem.h.
The desired heading direction (in rads), for behaviors 'beHeadDirection' and 'beDoorCrosing2'.
Definition at line 165 of file CReactiveNavigationSystem.h.
This is the desired "path" for passing a door (in GLOBAL coordinates!).
Definition at line 173 of file CReactiveNavigationSystem.h.
Definition at line 173 of file CReactiveNavigationSystem.h.
Critical zones:
Definition at line 197 of file CReactiveNavigationSystem.h.
synch::CCriticalSection mrpt::reactivenav::CReactiveNavigationSystem::m_critZoneNavigating [private] |
Definition at line 197 of file CReactiveNavigationSystem.h.
Definition at line 307 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::maxObstaclesHeight [private] |
Definition at line 230 of file CReactiveNavigationSystem.h.
Runtime estimation of execution period of the method.
Definition at line 249 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::meanExecutionTime [private] |
Definition at line 303 of file CReactiveNavigationSystem.h.
Definition at line 304 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::minObstaclesHeight [private] |
Definition at line 230 of file CReactiveNavigationSystem.h.
Will be false until the navigation end is sent, and it is reset with each new command.
Definition at line 193 of file CReactiveNavigationSystem.h.
This defines the 'behavior' of the navigator (see posible values in TNavigatorBehavior).
Definition at line 161 of file CReactiveNavigationSystem.h.
unsigned long mrpt::reactivenav::CReactiveNavigationSystem::nIteration [private] |
The iterations count.
Definition at line 245 of file CReactiveNavigationSystem.h.
Definition at line 305 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::prevSelPTG [private] |
Definition at line 157 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::prevV [private] |
Definition at line 157 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::prevW [private] |
Definition at line 157 of file CReactiveNavigationSystem.h.
std::vector<CParameterizedTrajectoryGenerator*> mrpt::reactivenav::CReactiveNavigationSystem::PTGs [private] |
The set of transformations to be used:
Definition at line 312 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::refDistance [private] |
Definition at line 224 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::robotMax_V_mps [private] |
Definition at line 226 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::robotMax_W_degps [private] |
Definition at line 227 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::ROBOTMODEL_DELAY [private] |
Definition at line 228 of file CReactiveNavigationSystem.h.
float mrpt::reactivenav::CReactiveNavigationSystem::ROBOTMODEL_TAU [private] |
Definition at line 228 of file CReactiveNavigationSystem.h.
std::string mrpt::reactivenav::CReactiveNavigationSystem::robotName [private] |
Definition at line 223 of file CReactiveNavigationSystem.h.
The robot 2D shape model.
Definition at line 261 of file CReactiveNavigationSystem.h.
Definition at line 297 of file CReactiveNavigationSystem.h.
Definition at line 220 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::times_HoloNav [private] |
Definition at line 301 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::times_TP_transformations [private] |
Definition at line 301 of file CReactiveNavigationSystem.h.
Definition at line 297 of file CReactiveNavigationSystem.h.
std::vector<vector_double> mrpt::reactivenav::CReactiveNavigationSystem::TP_Obstacles [private] |
Definition at line 298 of file CReactiveNavigationSystem.h.
std::vector<poses::CPoint2D,Eigen::aligned_allocator<poses::CPoint2D> > mrpt::reactivenav::CReactiveNavigationSystem::TP_Targets [private] |
Definition at line 299 of file CReactiveNavigationSystem.h.
std::vector<bool> mrpt::reactivenav::CReactiveNavigationSystem::valid_TP [private] |
Definition at line 302 of file CReactiveNavigationSystem.h.
Definition at line 234 of file CReactiveNavigationSystem.h.
Definition at line 236 of file CReactiveNavigationSystem.h.
std::vector<float> mrpt::reactivenav::CReactiveNavigationSystem::weights [private] |
Definition at line 229 of file CReactiveNavigationSystem.h.
Page generated by Doxygen 1.7.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |