See base class CAbstractPTGBasedReactive for a description and instructions of use.
This particular implementation assumes a 2D robot shape.
Publications:
Class history:
This class requires a number of parameters which are usually provided via an external config (".ini") file. Alternatively, a memory-only object can be used to avoid physical files, see mrpt::utils::CConfigFileMemory.
Next we provide a self-documented template config file:
# ------------------------------------------------------------------------ # Example configuration file for MRPT Reactive Navigation engine. # See C++ documentation: http://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_reactive_navigation_system.html # See ROS node documentation: http://wiki.ros.org/mrpt_reactivenav2d # ------------------------------------------------------------------------ [GLOBAL_CONFIG] # 0: Virtual Force Field # 1: Nearness Diagram (ND) HOLONOMIC_METHOD=1 ALARM_SEEMS_NOT_APPROACHING_TARGET_TIMEOUT=100 # (seconds) # Parameters for the "Nearness diagram" Holonomic method # ---------------------------------------------------- [ND_CONFIG] factorWeights=1.0 0.5 2.0 0.5 # 1: Free space # 2: Dist. in sectors # 3: Closer to target (euclidean) # 4: Hysteresis WIDE_GAP_SIZE_PERCENT = 0.25 MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.25 RISK_EVALUATION_SECTORS_PERCENT = 0.25 RISK_EVALUATION_DISTANCE = 0.5 # In normalized ps-meters [0,1] TARGET_SLOW_APPROACHING_DISTANCE = 0.8 # For stop gradually TOO_CLOSE_OBSTACLE = 0.03 # In normalized ps-meters # Parameters for the "VFF" Holonomic method # ---------------------------------------------------- [VFF_CONFIG] # Used to decrease speed gradually when the target is going to be reached TARGET_SLOW_APPROACHING_DISTANCE = 0.8 # Use it to control the relative weight of the target respect to the obstacles TARGET_ATTRACTIVE_FORCE = 7.5 # ---------------------------------------------------- # Parameters for navigation # ---------------------------------------------------- [ReactiveParams] weights=0.5 0.05 0.5 2.0 0.2 0.1 # 1: Free space # 2: Dist. in sectors # 3: Heading toward target # 4: Closer to target (euclidean) # 5: Hysteresis # 6: Security Distance DIST_TO_TARGET_FOR_SENDING_EVENT=0.6 # Minimum. distance to target for sending the end event. Set to 0 to send it just on navigation end MinObstaclesHeight=0.0 # Minimum coordinate in the "z" axis for an obstacle to be taken into account. MaxObstaclesHeight=1.40 # Maximum coordinate in the "z" axis for an obstacle to be taken into account. robotMax_V_mps = 1.0 # Speed limits robotMax_W_degps = 60 MAX_REFERENCE_DISTANCE = 8.0 # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths LUT_CELL_SIZE = 0.05 # Look-up-table cell size, or resolution (in meters) # The constant time of a first-order low-pass filter of outgoing speed commands, # i.e. can be used to impose a maximum acceleration. SPEEDFILTER_TAU = 0 # PTGs: See classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( http://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html)# refer to papers for details. #------------------------------------------------------------------------------ # Types: # 1 - Circular arcs \n" # 2 - alpha - A, Trajectories with asymptotical heading # 3 - C|C,S, R = vmax/wmax, Trajectories to move backward and then forward # 4 - C|C,s, like PTG 3, but if t > threshold -> v = w = 0 # 5 - CS, Trajectories with a minimum turning radius # 6 - alpha - SP, Trajectories built upon a spiral segment PTG_COUNT = 3 PTG0_Type = 1 PTG0_nAlfas = 121 PTG0_v_max_mps = 1.0 PTG0_w_max_gps = 60 PTG0_K = 1.0 PTG1_Type = 2 PTG1_nAlfas = 121 PTG1_v_max_mps = 1.0 PTG1_w_max_gps = 60 PTG1_cte_a0v_deg = 57 PTG1_cte_a0w_deg = 57 PTG2_Type = 1 PTG2_nAlfas = 121 PTG2_v_max_mps = 1.0 PTG2_w_max_gps = 60 PTG2_K = -1.0 # Default 2D robot shape for collision checks: (ignored in ROS, superseded by node parameters) RobotModel_shape2D_xs=-0.2 0.5 0.5 -0.2 RobotModel_shape2D_ys=0.3 0.3 -0.3 -0.3 ROBOTMODEL_DELAY=0 # (un-used param, must be present for compat. with old mrpt versions) ROBOTMODEL_TAU=0 # (un-used param, must be present for compat. with old mrpt versions)
Definition at line 44 of file nav/reactive/CReactiveNavigationSystem.h.
#include <mrpt/nav/reactive/CReactiveNavigationSystem.h>
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) | |
See docs in ctor of base class. More... | |
virtual | ~CReactiveNavigationSystem () |
Destructor. More... | |
void | loadConfigFile (const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni) |
Reload the configuration from a file. More... | |
void | loadConfigFile (const mrpt::utils::CConfigFileBase &ini) |
Reload the configuration from a file. More... | |
void | changeRobotShape (const math::CPolygon &shape) |
Change the robot shape, which is taken into account for collision grid building. More... | |
virtual size_t | getPTG_count () const |
Returns the number of different PTGs that have been setup. More... | |
virtual CParameterizedTrajectoryGenerator * | getPTG (size_t i) |
Gets the i'th PTG. More... | |
void | initialize () |
Must be called for loading collision grids, or the first navigation command may last a long time to be executed. More... | |
void | setHolonomicMethod (THolonomicMethod method, const mrpt::utils::CConfigFileBase &cfgBase) |
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. More... | |
void | loadHolonomicMethodConfig (const mrpt::utils::CConfigFileBase &ini, const std::string §ion) |
Just loads the holonomic method params from the given config source. More... | |
virtual void | navigate (const TNavigationParams *params) |
Start navigation: More... | |
void | getLastLogRecord (CLogFileRecord &o) |
Provides a copy of the last log record with information about execution. More... | |
void | enableKeepLogRecords (bool enable=true) |
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() More... | |
void | enableLogFile (bool enable) |
Enables/disables saving log files. More... | |
void | enableTimeLog (bool enable=true) |
Enables/disables the detailed time logger (default:disabled upon construction) When enabled, a report will be dumped to std::cout upon destruction. More... | |
const mrpt::utils::CTimeLogger & | getTimeLogger () const |
Gives access to a const-ref to the internal time logger. More... | |
void | cancel () |
Cancel current navegacion. More... | |
void | resume () |
Continues with suspended navigation. More... | |
void | navigationStep () |
This method must be called periodically in order to effectively run the navigation. More... | |
virtual void | suspend () |
Suspend current navegation. More... | |
TState | getCurrentState () const |
Returns the current navigator state. More... | |
Static Public Member Functions | |
static void | printf_debug (const char *frmt,...) |
Sends a formated text to "debugOut" if not NULL, or to cout otherwise. More... | |
Protected Member Functions | |
void | performNavigationStep () |
The main method for the navigator. More... | |
void | doEmergencyStop (const char *msg) |
Stops the robot and set navigation state to error. More... | |
void | STEP5_PTGEvaluator (THolonomicMovement &holonomicMovement, const std::vector< float > &in_TPObstacles, const mrpt::math::TPose2D &WS_Target, const mrpt::math::TPoint2D &TP_Target, CLogFileRecord::TInfoPerPTG &log) |
Scores holonomicMovement. More... | |
virtual void | STEP7_GenerateSpeedCommands (const THolonomicMovement &in_movement) |
void | preDestructor () |
To be called during children destructors to assure thread-safe destruction, and free of shared objects. More... | |
Protected Attributes | |
std::vector< CAbstractHolonomicReactiveMethod * > | m_holonomicMethod |
The holonomic navigation algorithm (one object per PTG, so internal states are maintained) More... | |
mrpt::utils::CStream * | m_logFile |
The current log file stream, or NULL if not being used. More... | |
bool | m_enableKeepLogRecords |
See enableKeepLogRecords. More... | |
CLogFileRecord | lastLogRecord |
The last log. More... | |
float | last_cmd_v |
float | last_cmd_w |
float | new_cmd_v |
float | new_cmd_w |
Speed actual and last commands: More... | |
bool | navigationEndEventSent |
Will be false until the navigation end is sent, and it is reset with each new command. More... | |
synch::CCriticalSection | m_critZoneLastLog |
synch::CCriticalSection | m_critZoneNavigating |
Critical zones. More... | |
bool | m_enableConsoleOutput |
Enables / disables the console debug output. More... | |
bool | m_init_done |
Whether loadConfigFile() has been called or not. More... | |
mrpt::utils::CTicTac | timerForExecutionPeriod |
std::string | ptg_cache_files_directory |
(Default: ".") More... | |
std::string | robotName |
Robot name. More... | |
float | refDistance |
"D_{max}" in papers. More... | |
float | colGridRes |
CollisionGrid resolution. More... | |
float | robotMax_V_mps |
Max. linear speed (m/s) More... | |
float | robotMax_W_degps |
Max. angular speed (deg/s) More... | |
float | SPEEDFILTER_TAU |
Time constant for the low-pass filter applied to the speed commands. More... | |
std::vector< float > | weights |
length: 6 [0,5] More... | |
float | secureDistanceStart |
In normalized distances, the start and end of a ramp function that scales the velocity output from the holonomic navigator: More... | |
float | secureDistanceEnd |
float | DIST_TO_TARGET_FOR_SENDING_EVENT |
float | meanExecutionPeriod |
Runtime estimation of execution period of the method. More... | |
mrpt::utils::CTimeLogger | m_timelogger |
A complete time logger. More... | |
float | badNavAlarm_minDistTarget |
For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... More... | |
mrpt::system::TTimeStamp | badNavAlarm_lastMinDistTime |
float | badNavAlarm_AlarmTimeout |
bool | m_collisionGridsMustBeUpdated |
TState | m_navigationState |
Current internal state of navigator: More... | |
TNavigationParams * | m_navigationParams |
Current navigation parameters. More... | |
CReactiveInterfaceImplementation & | m_robot |
The navigator-robot interface. More... | |
Variables for CReactiveNavigationSystem::performNavigationStep | |
mrpt::utils::CTicTac | totalExecutionTime |
mrpt::utils::CTicTac | executionTime |
mrpt::utils::CTicTac | tictac |
float | meanExecutionTime |
float | meanTotalExecutionTime |
Private Member Functions | |
virtual void | STEP1_CollisionGridsBuilder () |
virtual bool | STEP2_SenseObstacles () |
Return false on any fatal error. More... | |
virtual void | STEP3_WSpaceToTPSpace (const size_t ptg_idx, std::vector< float > &out_TPObstacles) |
Builds TP-Obstacles from Workspace obstacles for the given PTG. More... | |
virtual void | loggingGetWSObstaclesAndShape (CLogFileRecord &out_log) |
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep. More... | |
Private Attributes | |
float | minObstaclesHeight |
float | maxObstaclesHeight |
math::CPolygon | m_robotShape |
The robot 2D shape model. More... | |
std::vector< CParameterizedTrajectoryGenerator * > | PTGs |
The set of transformations to be used: More... | |
mrpt::maps::CSimplePointsMap | m_WS_Obstacles |
The obstacle points, as seen from the local robot frame. More... | |
|
inherited |
The different states for the navigation system.
Enumerator | |
---|---|
IDLE | |
NAVIGATING | |
SUSPENDED | |
NAV_ERROR |
Definition at line 145 of file CAbstractReactiveNavigationSystem.h.
mrpt::nav::CReactiveNavigationSystem::CReactiveNavigationSystem | ( | CReactiveInterfaceImplementation & | react_iterf_impl, |
bool | enableConsoleOutput = true , |
||
bool | enableLogFile = false |
||
) |
See docs in ctor of base class.
|
virtual |
Destructor.
|
inherited |
Cancel current navegacion.
void mrpt::nav::CReactiveNavigationSystem::changeRobotShape | ( | const math::CPolygon & | shape | ) |
Change the robot shape, which is taken into account for collision grid building.
|
protectedinherited |
Stops the robot and set navigation state to error.
|
inlineinherited |
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord()
Definition at line 106 of file CAbstractPTGBasedReactive.h.
|
inherited |
Enables/disables saving log files.
|
inlineinherited |
Enables/disables the detailed time logger (default:disabled upon construction) When enabled, a report will be dumped to std::cout upon destruction.
Definition at line 115 of file CAbstractPTGBasedReactive.h.
References MRPT_UNUSED_PARAM.
|
inlineinherited |
Returns the current navigator state.
Definition at line 154 of file CAbstractReactiveNavigationSystem.h.
|
inherited |
Provides a copy of the last log record with information about execution.
o | An object where the log will be stored into. |
|
inlinevirtual |
Gets the i'th PTG.
Implements mrpt::nav::CAbstractPTGBasedReactive.
Definition at line 77 of file nav/reactive/CReactiveNavigationSystem.h.
References ASSERT_.
|
inlinevirtual |
Returns the number of different PTGs that have been setup.
Implements mrpt::nav::CAbstractPTGBasedReactive.
Definition at line 74 of file nav/reactive/CReactiveNavigationSystem.h.
|
inlineinherited |
Gives access to a const-ref to the internal time logger.
Definition at line 121 of file CAbstractPTGBasedReactive.h.
|
inherited |
Must be called for loading collision grids, or the first navigation command may last a long time to be executed.
Internally, it just calls STEP1_CollisionGridsBuilder().
void mrpt::nav::CReactiveNavigationSystem::loadConfigFile | ( | const mrpt::utils::CConfigFileBase & | ini, |
const mrpt::utils::CConfigFileBase & | robotIni | ||
) |
Reload the configuration from a file.
See details in CReactiveNavigationSystem docs.
[in] | ini | The main source of configuration parameters. |
[in] | robotIni | Deprecated (kept for backwards compatibility). It is recommended to use the newer loadConfigFile() method with one argument in new code. |
void mrpt::nav::CReactiveNavigationSystem::loadConfigFile | ( | const mrpt::utils::CConfigFileBase & | ini | ) |
Reload the configuration from a file.
See details in CReactiveNavigationSystem docs.
|
inherited |
Just loads the holonomic method params from the given config source.
|
privatevirtual |
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep.
Implements mrpt::nav::CAbstractPTGBasedReactive.
|
virtualinherited |
Start navigation:
[in] | params | Pointer to structure with navigation info (its contents will be copied, so the original can be freely destroyed upon return.) |
Implements mrpt::nav::CAbstractReactiveNavigationSystem.
|
inherited |
This method must be called periodically in order to effectively run the navigation.
|
protectedvirtualinherited |
The main method for the navigator.
Implements mrpt::nav::CAbstractReactiveNavigationSystem.
|
protectedinherited |
To be called during children destructors to assure thread-safe destruction, and free of shared objects.
|
staticinherited |
Sends a formated text to "debugOut" if not NULL, or to cout otherwise.
Referenced by mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute().
|
inherited |
Continues with suspended navigation.
|
inherited |
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.
|
privatevirtual |
Implements mrpt::nav::CAbstractPTGBasedReactive.
|
privatevirtual |
Return false on any fatal error.
Implements mrpt::nav::CAbstractPTGBasedReactive.
|
privatevirtual |
Builds TP-Obstacles from Workspace obstacles for the given PTG.
"out_TPObstacles" is already initialized to the proper length and maximum collision-free distance for each "k" trajectory index. Distances are in "pseudo-meters". They will be normalized automatically to [0,1] upon return.
Implements mrpt::nav::CAbstractPTGBasedReactive.
|
protectedinherited |
Scores holonomicMovement.
|
protectedvirtualinherited |
|
virtualinherited |
Suspend current navegation.
|
protectedinherited |
Definition at line 199 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 198 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...
Definition at line 197 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
CollisionGrid resolution.
Definition at line 167 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 192 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 208 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 155 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 155 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
The last log.
Definition at line 154 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 201 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 157 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Critical zones.
Definition at line 157 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Enables / disables the console debug output.
Definition at line 159 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
See enableKeepLogRecords.
Definition at line 153 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
Definition at line 151 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Whether loadConfigFile() has been called or not.
Definition at line 160 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
The current log file stream, or NULL if not being used.
Definition at line 152 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Current navigation parameters.
Definition at line 164 of file CAbstractReactiveNavigationSystem.h.
|
protectedinherited |
Current internal state of navigator:
Definition at line 163 of file CAbstractReactiveNavigationSystem.h.
|
protectedinherited |
The navigator-robot interface.
Definition at line 167 of file CAbstractReactiveNavigationSystem.h.
|
private |
The robot 2D shape model.
Definition at line 91 of file nav/reactive/CReactiveNavigationSystem.h.
|
protectedinherited |
A complete time logger.
Definition at line 194 of file CAbstractPTGBasedReactive.h.
|
private |
The obstacle points, as seen from the local robot frame.
Definition at line 111 of file nav/reactive/CReactiveNavigationSystem.h.
|
private |
Definition at line 88 of file nav/reactive/CReactiveNavigationSystem.h.
|
protectedinherited |
Runtime estimation of execution period of the method.
Definition at line 193 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 209 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 209 of file CAbstractPTGBasedReactive.h.
|
private |
Definition at line 88 of file nav/reactive/CReactiveNavigationSystem.h.
|
protectedinherited |
Will be false until the navigation end is sent, and it is reset with each new command.
Definition at line 156 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 155 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Speed actual and last commands:
Definition at line 155 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
(Default: ".")
Definition at line 164 of file CAbstractPTGBasedReactive.h.
|
private |
The set of transformations to be used:
Definition at line 95 of file nav/reactive/CReactiveNavigationSystem.h.
|
protectedinherited |
"D_{max}" in papers.
Definition at line 166 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Max. linear speed (m/s)
Definition at line 168 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Max. angular speed (deg/s)
Definition at line 169 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Robot name.
Definition at line 165 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 189 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
In normalized distances, the start and end of a ramp function that scales the velocity output from the holonomic navigator:
Definition at line 189 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Time constant for the low-pass filter applied to the speed commands.
Definition at line 170 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 208 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 161 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 208 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
length: 6 [0,5]
Definition at line 171 of file CAbstractPTGBasedReactive.h.
Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN: at Mon Aug 15 11:50:21 UTC 2016 |