See base class CAbstractPTGBasedReactive for a description and instructions of use.
This particular implementation assumes a 3D (or "2.5D") robot shape model, build as a vertical stack of "2D slices".
Paper describing the method:
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 2.5 Reactive Navigation engine. # See C++ documentation: http://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_reactive_navigation_system3_d.html # This .ini file is missing information about the equipped sensors and the Short-Term memory # For more detailed information, please see the App "ReactiveNav3D-Demo" within MRPT # ------------------------------------------------------------------------------------------ [ROBOT_CONFIG] Name = MyRobot #Optional #Indicate the geometry of the robot as a set of prisms. #Format - (LEVELX_HEIGHT, LEVELX_VECTORX, LEVELX_VECTORY) #Number of height levels used to model the robot HEIGHT_LEVELS = 2 #Geometrical description of each level LEVEL1_HEIGHT = 0.6 LEVEL1_VECTORX = 0.3 0.3 -0.3 -0.3 LEVEL1_VECTORY = -0.3 0.3 0.3 -0.3 LEVEL2_HEIGHT = 0.4 LEVEL2_VECTORX = 0.05 0.05 -0.05 -0.05 LEVEL2_VECTORY = -0.1 0.1 0.1 -0.1 # ---------------------------------------------------- # Parameters for navigation # ---------------------------------------------------- [NAVIGATION_CONFIG] # 0: Virtual Force Field (VFF) # 1: Nearness Diagram (ND) HOLONOMIC_METHOD = 1 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.5 # Minimum. distance to target for sending the end event. Set to 0 to send it just on navigation end VMAX_MPS = 0.70 # Translational speed limit (m/s) WMAX_DEGPS = 60 # Rotational speed limit (deg/s) 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 MAX_DISTANCE_PTG = 2 # Marks the maximum distance regarded by the reactive navigator (m) GRID_RESOLUTION = 0.02 # Resolutions used to build the collision_grid (m) SPEEDFILTER_TAU = 0.1 # The 'TAU' time constant of a 1st order lowpass filter for speed commands (s) # 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 # Parameters for the "Nearness diagram" Holonomic method # -------------------------------------------------------- [ND_CONFIG] factorWeights = 1.0 2.0 0.5 1.0 # 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.7 # 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
Definition at line 46 of file nav/reactive/CReactiveNavigationSystem3D.h.
#include <mrpt/nav/reactive/CReactiveNavigationSystem3D.h>
Classes | |
struct | TPTGmultilevel |
A set of PTGs of the same type, one per "height level". More... | |
Public Types | |
enum | TState { IDLE =0, NAVIGATING, SUSPENDED, NAV_ERROR } |
The different states for the navigation system. More... | |
Public Member Functions | |
CReactiveNavigationSystem3D (CReactiveInterfaceImplementation &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false) | |
See docs in ctor of base class. More... | |
virtual | ~CReactiveNavigationSystem3D () |
Destructor. More... | |
void | loadConfigFile (const mrpt::utils::CConfigFileBase &ini) |
Reload the configuration from a file. More... | |
void | changeRobotShape (TRobotShape robotShape) |
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 | 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 | |
mrpt::maps::CSimplePointsMap | m_WS_Obstacles_unsorted |
The unsorted set of obstacles from the sensors. More... | |
std::vector< mrpt::maps::CSimplePointsMap > | m_WS_Obstacles_inlevels |
One point cloud per 2.5D robot-shape-slice, coordinates relative to the robot local frame. More... | |
TRobotShape | m_robotShape |
The robot 3D shape model. More... | |
std::vector< TPTGmultilevel > | m_ptgmultilevel |
The set of PTG-transformations to be used: 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::CReactiveNavigationSystem3D::CReactiveNavigationSystem3D | ( | 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::CReactiveNavigationSystem3D::changeRobotShape | ( | TRobotShape | robotShape | ) |
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 70 of file nav/reactive/CReactiveNavigationSystem3D.h.
References ASSERT_.
|
inlinevirtual |
Returns the number of different PTGs that have been setup.
Implements mrpt::nav::CAbstractPTGBasedReactive.
Definition at line 67 of file nav/reactive/CReactiveNavigationSystem3D.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::CReactiveNavigationSystem3D::loadConfigFile | ( | const mrpt::utils::CConfigFileBase & | ini | ) |
Reload the configuration from a file.
See CReactiveNavigationSystem3D for details.
|
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 198 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 197 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 196 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
CollisionGrid resolution.
Definition at line 166 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 191 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 207 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 200 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.
|
private |
The set of PTG-transformations to be used:
Definition at line 103 of file nav/reactive/CReactiveNavigationSystem3D.h.
|
protectedinherited |
The navigator-robot interface.
Definition at line 167 of file CAbstractReactiveNavigationSystem.h.
|
private |
The robot 3D shape model.
Definition at line 100 of file nav/reactive/CReactiveNavigationSystem3D.h.
|
protectedinherited |
A complete time logger.
Definition at line 193 of file CAbstractPTGBasedReactive.h.
|
private |
One point cloud per 2.5D robot-shape-slice, coordinates relative to the robot local frame.
Definition at line 96 of file nav/reactive/CReactiveNavigationSystem3D.h.
|
private |
The unsorted set of obstacles from the sensors.
Definition at line 95 of file nav/reactive/CReactiveNavigationSystem3D.h.
|
protectedinherited |
Runtime estimation of execution period of the method.
Definition at line 192 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 208 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 208 of file CAbstractPTGBasedReactive.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 |
"D_{max}" in papers.
Definition at line 165 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Max. linear speed (m/s)
Definition at line 167 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Max. angular speed (deg/s)
Definition at line 168 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Robot name.
Definition at line 164 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 188 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 188 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Time constant for the low-pass filter applied to the speed commands.
Definition at line 169 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 207 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 161 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
Definition at line 207 of file CAbstractPTGBasedReactive.h.
|
protectedinherited |
length: 6 [0,5]
Definition at line 170 of file CAbstractPTGBasedReactive.h.
Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016 |