See base class CAbstractPTGBasedReactive for a description and instructions of use.
This particular implementation assumes a 2D robot shape which can be polygonal or circular (depending on the selected PTGs).
Publications:
Class history:
- 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.
- DEC/2013: Code refactoring between this class and CAbstractHolonomicReactiveMethod
- FEB/2017: Refactoring of all parameters for a consistent organization in sections by class names (MRPT 1.5.0)
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.
A template config file can be generated at any moment by the user by calling saveConfigFile() with a default-constructed object.
Next we provide a self-documented template config file; or see it online: https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini
# ------------------------------------------------------------------------
# Example configuration file for MRPT (>=1.5) Reactive Navigation engine.
# See C++ documentation: http://reference.mrpt.org/devel/classmrpt_1_1nav_1_1_c_reactive_navigation_system.html
# See ROS node documentation: http://wiki.ros.org/mrpt_reactivenav2d
# ------------------------------------------------------------------------
# Max linear vel (m/s):
@define ROBOT_MAX_V 1.0
# Max angular vel (deg/s):
@define ROBOT_MAX_W 60.0
# Max distance to "foresee" obstacles (m):
@define NAV_MAX_REF_DIST 10.0
[CAbstractNavigator]
dist_to_target_for_sending_event = 0.000000 // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request.
alarm_seems_not_approaching_target_timeout = 30.000000 // navigator timeout (seconds) [Default=30 sec]
dist_check_target_is_blocked = 2.0 // When closer than this distance, check if the target is blocked to abort navigation with an error
hysteresis_check_target_is_blocked = 3 // How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event
[CWaypointsNavigator]
max_distance_to_allow_skip_waypoint = -1.000000 // Max distance to `foresee` waypoints [meters]. (<0: unlimited)
min_timesteps_confirm_skip_waypoints = 1 // Min timesteps a `future` waypoint must be seen as reachable to become the active one.
waypoint_angle_tolerance = 5.0 // Angular error tolerance for waypoints with an assigned heading [deg]
multitarget_look_ahead = 0 // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none).
[CAbstractPTGBasedReactive]
robotMax_V_mps = ${ROBOT_MAX_V} // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set]
robotMax_W_degps = ${ROBOT_MAX_W} // Max. angular speed (rad/s) [Default=-1 (not set), will raise exception if needed and not set]
#robotMinCurvRadius = -1.000000 // Min. radius of curvature of paths (m) [Default=-1 (not set), will raise exception if needed and not set]
holonomic_method = CHolonomicFullEval // C++ class name of the holonomic navigation method to run in the transformed TP-Space.
# List of known classes:
# - `CHolonomicFullEval`
# - `CHolonomicND`
# - `CHolonomicVFF`
motion_decider_method = CMultiObjMotionOpt_Scalarization // C++ class name of the motion decider method.
# List of known classes:
# - `CMultiObjMotionOpt_Scalarization`
ref_distance = ${NAV_MAX_REF_DIST} // Maximum distance up to obstacles will be considered (D_{max} in papers).
#speedfilter_tau = 0.000000 // Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering)
secure_distance_start = 0.050000 // In normalized distance [0,1], start/end of a ramp function that scales the holonomic navigator output velocity.
secure_distance_end = 0.150000 // In normalized distance [0,1], start/end of a ramp function that scales the holonomic navigator output velocity.
use_delays_model = false // Whether to use robot pose inter/extrapolation to improve accuracy (Default:false)
max_distance_predicted_actual_path = 0.150000 // Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05)
min_normalized_free_space_for_ptg_continuation = 0.200000 // Min normalized dist [0,1] after current pose in a PTG continuation to allow it.
enable_obstacle_filtering = true // Enabled obstacle filtering (params in its own section)
evaluate_clearance = true
[CPointCloudFilterByDistance]
min_dist = 0.100000
angle_tolerance = 5.000000
too_old_seconds = 1.000000
previous_keyframes = 1 // (Default: 1) How many previous keyframes will be compared with the latest pointcloud.
max_deletion_ratio = 0.400000 // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it'd be too suspicious and may indicate a failure of this filter.
[CHolonomicFullEval]
# [0]=Free space
# [1]=Dist. in sectors
# [2]=Closer to target (Euclidean)
# [3]=Hysteresis
# [4]=clearance along path
factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5 ]
factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 ]
TOO_CLOSE_OBSTACLE = 0.150000 // Directions with collision-free distances below this threshold are not elegible.
TARGET_SLOW_APPROACHING_DISTANCE = 0.100000 // Start to reduce speed when closer than this to target.
OBSTACLE_SLOW_DOWN_DISTANCE = 0.150000 // Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max distance)
HYSTERESIS_SECTOR_COUNT = 5.000000 // Range of `sectors` (directions) for hysteresis over successive timesteps
LOG_SCORE_MATRIX = false // Save the entire score matrix in log files
clearance_threshold_ratio = 0.10 // Ratio [0,1], times path_count, gives the minimum number of paths at each side of a target direction to be accepted as desired direction
gap_width_ratio_threshold = 0.20 // Ratio [0,1], times path_count, gives the minimum gap width to accept a direct motion towards target.
PHASE_COUNT = 3 // Number of evaluation phases to run (params for each phase below)
PHASE1_FACTORS = [1 2] // Indices of the factors above to be considered in this phase
PHASE1_THRESHOLD = 0.5 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`)
PHASE2_FACTORS = [4 0] // Indices of the factors above to be considered in this phase
PHASE2_THRESHOLD = 0.6 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`)
PHASE3_FACTORS = [0 2] // Indices of the factors above to be considered in this phase
PHASE3_THRESHOLD = 0.7 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`)
[CHolonomicND]
WIDE_GAP_SIZE_PERCENT = 0.250000
MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.250000
RISK_EVALUATION_SECTORS_PERCENT = 0.100000
RISK_EVALUATION_DISTANCE = 0.400000 // In normalized ps-meters [0,1]
TOO_CLOSE_OBSTACLE = 0.150000 // For stopping gradually
TARGET_SLOW_APPROACHING_DISTANCE = 0.600000 // In normalized ps-meters
factorWeights = 1.00 0.50 2.00 0.40 // [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis
[CHolonomicVFF]
TARGET_SLOW_APPROACHING_DISTANCE = 0.100000 // For stopping gradually
TARGET_ATTRACTIVE_FORCE = 20.000000 // Dimension-less (may have to be tuned depending on the density of obstacle sampling)
[CMultiObjectiveMotionOptimizerBase]
# Next follows a list of `score%i_{name,formula}` pairs for i=1,...,N
# Each one defines an exprtk formula for one of the scores that will be evaluated for each candidate movement.
# Multiobjective optimizers will then use those scores to select the best candidate,
# possibly using more parameters that follow below.
# See list of all available variables in documentation of mrpt::nav::CAbstractPTGBasedReactive at http://reference.mrpt.org/devel/classmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive.html
score1_name = target_distance
score1_formula = \
var effective_trg_d_norm := max(0,target_d_norm-move_cur_d); \
if(collision_free_distance>effective_trg_d_norm, \
1/(1+effective_trg_d_norm^2), \
0)
score2_name = collision_free_distance_score
score2_formula = \
var effective_trg_d_norm := max(0,target_d_norm-move_cur_d); \
if (collision_free_distance>(effective_trg_d_norm+0.05), \
1.0, \
collision_free_distance)
score3_name = euclidean_nearness
score3_formula = 1/(1+dist_eucl_min^2)
score4_name = hysteresis_score
score4_formula = hysteresis
score5_name = path_index_near_target
score5_formula = \
var dif:=abs(target_k-move_k); \
if (dif>(num_paths/2)) \
{ \
dif:=num_paths-dif; \
}; \
exp(-abs(dif / (num_paths/10.0)));
score6_name = ptg_priority_score
score6_formula = ptg_priority
# Next follows a list of `movement_assert%i` exprtk expressions for i=1,...,N
# defining expressions for conditions that any candidate movement must fulfill
# in order to get through the evaluation process. *All* assert conditions must be satisfied.
#movement_assert1 = XXX
# Comma-separated list of scores to normalize so the highest is 1.0.
scores_to_normalize =
#target_distance
[CMultiObjMotionOpt_Scalarization]
# A formula that takes all/a subset of scores and generates a scalar global score.
scalar_score_formula = ptg_priority_score*( \
0.3*target_distance + \
0.5*collision_free_distance_score + \
8.0*euclidean_nearness + \
0.1 * hysteresis_score + \
0.2*path_index_near_target \
)
[CReactiveNavigationSystem]
min_obstacles_height = 0.000000 // Minimum `z` coordinate of obstacles to be considered fo collision checking
max_obstacles_height = 10.000000 // Maximum `z` coordinate of obstacles to be considered fo collision checking
# 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.
#------------------------------------------------------------------------------
PTG_COUNT = 3
PTG0_Type = CPTG_DiffDrive_C
PTG0_resolution = 0.05 # Look-up-table cell size or resolution (in meters)
PTG0_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths
PTG0_num_paths= 121
PTG0_v_max_mps = ${ROBOT_MAX_V}
PTG0_w_max_dps = ${ROBOT_MAX_W}
PTG0_K = 1.0
PTG0_score_priority = 1.0
PTG1_Type = CPTG_DiffDrive_alpha
PTG1_resolution = 0.05 # Look-up-table cell size or resolution (in meters)
PTG1_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths
PTG1_num_paths = 121
PTG1_v_max_mps = ${ROBOT_MAX_V}
PTG1_w_max_dps = ${ROBOT_MAX_W}
PTG1_cte_a0v_deg = 57
PTG1_cte_a0w_deg = 57
PTG1_score_priority = 1.0
PTG2_Type = CPTG_DiffDrive_C
PTG2_resolution = 0.05 # Look-up-table cell size or resolution (in meters)
PTG2_refDistance= ${NAV_MAX_REF_DIST} # Maximum distance to build PTGs (in meters), i.e. the visibility "range" of tentative paths
PTG2_num_paths = 121
PTG2_v_max_mps = ${ROBOT_MAX_V}
PTG2_w_max_dps = ${ROBOT_MAX_W}
PTG2_K = -1.0
PTG2_score_priority = 0.5
# Default 2D robot shape for collision checks: (ignored in ROS, superseded by node parameters)
# Each PTG will use only one of either (a) polygonal 2D shape or, (b) radius of a circular shape
RobotModel_shape2D_xs=-0.2 0.1 0.1 -0.2
RobotModel_shape2D_ys=0.1 0.1 -0.1 -0.1
RobotModel_circular_shape_radius = 0.5
- See also
- CAbstractNavigator, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod
Definition at line 47 of file nav/reactive/CReactiveNavigationSystem.h.
|
| CReactiveNavigationSystem (CRobot2NavInterface &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false, const std::string &logFileDirectory=std::string("./reactivenav.logs")) |
| See docs in ctor of base class. More...
|
|
virtual | ~CReactiveNavigationSystem () |
| Destructor. More...
|
|
void | changeRobotShape (const mrpt::math::CPolygon &shape) |
| Defines the 2D polygonal robot shape, used for some PTGs for collision checking. More...
|
|
void | changeRobotCircularShapeRadius (const double R) |
| Defines the 2D circular robot shape radius, used for some PTGs for collision checking. More...
|
|
virtual size_t | getPTG_count () const MRPT_OVERRIDE |
| Returns the number of different PTGs that have been setup. More...
|
|
virtual CParameterizedTrajectoryGenerator * | getPTG (size_t i) MRPT_OVERRIDE |
| Gets the i'th PTG. More...
|
|
virtual const CParameterizedTrajectoryGenerator * | getPTG (size_t i) const MRPT_OVERRIDE |
| Gets the i'th PTG. More...
|
|
virtual bool | checkCollisionWithLatestObstacles (const mrpt::math::TPose2D &relative_robot_pose) const MRPT_OVERRIDE |
| Checks whether the robot shape, when placed at the given pose (relative to the current pose), is colliding with any of the latest known obstacles. More...
|
|
virtual void | loadConfigFile (const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE |
| Loads all params from a file. More...
|
|
virtual void | saveConfigFile (mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE |
| Saves all current options to a config file. More...
|
|
void | initialize () MRPT_OVERRIDE |
| Must be called for loading collision grids, or the first navigation command may last a long time to be executed. More...
|
|
void | setHolonomicMethod (const std::string &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 | setHolonomicMethod (THolonomicMethod method, const mrpt::utils::CConfigFileBase &cfgBase) |
|
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 | setLogFileDirectory (const std::string &sDir) |
| Changes the prefix for new log files. More...
|
|
std::string | getLogFileDirectory () const |
|
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...
|
|
const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & | getCurrentRobotSpeedLimits () const |
| Get the current, global (honored for all PTGs) robot speed limits. More...
|
|
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & | changeCurrentRobotSpeedLimits () |
| Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a structure that holds those limits. More...
|
|
void | setTargetApproachSlowDownDistance (const double dist) |
| Changes this parameter in all inner holonomic navigator instances [m]. More...
|
|
double | getTargetApproachSlowDownDistance () const |
| Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in all of them?) More...
|
|
virtual void | navigationStep () MRPT_OVERRIDE |
| This method must be called periodically in order to effectively run the navigation. More...
|
|
virtual void | cancel () MRPT_OVERRIDE |
| Cancel current navegation. More...
|
|
bool | isRelativePointReachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const |
| Returns true if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range for the existing PTGs, etc. More...
|
|
const mrpt::utils::CTimeLogger & | getDelaysTimeLogger () const |
| Gives access to a const-ref to the internal time logger used to estimate delays. More...
|
|
|
virtual void | navigateWaypoints (const TWaypointSequence &nav_request) |
| Waypoint navigation request. More...
|
|
virtual void | getWaypointNavStatus (TWaypointStatusSequence &out_nav_status) const |
| Get a copy of the control structure which describes the progress status of the waypoint navigation. More...
|
|
TWaypointStatusSequence | getWaypointNavStatus () const |
| Get a copy of the control structure which describes the progress status of the waypoint navigation. More...
|
|
|
virtual void | loggingGetWSObstaclesAndShape (CLogFileRecord &out_log) MRPT_OVERRIDE |
| Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep. More...
|
|
void | STEP3_WSpaceToTPSpace (const size_t ptg_idx, std::vector< double > &out_TPObstacles, mrpt::nav::ClearanceDiagram &out_clearance, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, const bool eval_clearance) MRPT_OVERRIDE |
| Builds TP-Obstacles from Workspace obstacles for the given PTG. More...
|
|
virtual void | performNavigationStep () MRPT_OVERRIDE |
| The main method for the navigator. More...
|
|
virtual bool | impl_waypoint_is_reachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const MRPT_OVERRIDE |
| Implements the way to waypoint is free function in children classes: true must be returned if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range, etc. More...
|
|
bool | STEP2_SenseObstacles () |
|
void | calc_move_candidate_scores (TCandidateMovementPTG &holonomicMovement, const std::vector< double > &in_TPObstacles, const mrpt::nav::ClearanceDiagram &in_clearance, const std::vector< mrpt::math::TPose2D > &WS_Targets, const std::vector< PTGTarget > &TP_Targets, CLogFileRecord::TInfoPerPTG &log, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, const mrpt::math::TPose2D &relPoseVelCmd_NOP, const unsigned int ptg_idx4weights, const mrpt::system::TTimeStamp tim_start_iteration) |
| Scores holonomicMovement. More...
|
|
virtual double | generate_vel_cmd (const TCandidateMovementPTG &in_movement, mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd) |
| Return the [0,1] velocity scale of raw PTG cmd_vel. More...
|
|
void | STEP8_GenerateLogRecord (CLogFileRecord &newLogRec, const std::vector< mrpt::math::TPose2D > &relTargets, int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd, int nPTGs, const bool best_is_NOP_cmdvel, const math::TPose2D &rel_cur_pose_wrt_last_vel_cmd_NOP, const math::TPose2D &rel_pose_PTG_origin_wrt_sense_NOP, const double executionTimeValue, const double tim_changeSpeed, const mrpt::system::TTimeStamp &tim_start_iteration) |
|
void | preDestructor () |
| To be called during children destructors to assure thread-safe destruction, and free of shared objects. More...
|
|
virtual void | onStartNewNavigation () MRPT_OVERRIDE |
| Called whenever a new navigation has been started. More...
|
|
void | build_movement_candidate (CParameterizedTrajectoryGenerator *ptg, const size_t indexPTG, const std::vector< mrpt::math::TPose2D > &relTargets, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, TInfoPerPTG &ipf, TCandidateMovementPTG &holonomicMovement, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, mrpt::nav::CAbstractHolonomicReactiveMethod *holoMethod, const mrpt::system::TTimeStamp tim_start_iteration, const TNavigationParams &navp=TNavigationParams(), const mrpt::math::TPose2D &relPoseVelCmd_NOP=mrpt::poses::CPose2D()) |
|
virtual bool | checkHasReachedTarget (const double targetDist) const MRPT_OVERRIDE |
| Default implementation: check if target_dist is below the accepted distance. More...
|
|
virtual void | waypoints_navigationStep () |
| The waypoints-specific part of navigationStep() More...
|
|
bool | waypoints_isAligning () const |
|
void | dispatchPendingNavEvents () |
|
void | updateCurrentPoseAndSpeeds () |
| Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly. More...
|
|
virtual void | performNavigationStepNavigating (bool call_virtual_nav_method=true) |
| Factorization of the part inside navigationStep(), for the case of state being NAVIGATING. More...
|
|
void | doEmergencyStop (const std::string &msg) |
| Stops the robot and set navigation state to error. More...
|
|
virtual bool | changeSpeeds (const mrpt::kinematics::CVehicleVelCmd &vel_cmd) |
| Default: forward call to m_robot.changeSpeed(). Can be overriden. More...
|
|
virtual bool | changeSpeedsNOP () |
| Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden. More...
|
|
virtual bool | stop (bool isEmergencyStop) |
| Default: forward call to m_robot.stop(). Can be overriden. More...
|
|