From 1a5ad432278293c718f9d6ec5f49d14b6ed03af2 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Mar 2017 23:23:18 +0100 Subject: [PATCH] Update docs on rnav score variable names Progress with #193 --- .../nav/reactive/CAbstractPTGBasedReactive.h | 21 +++++++++++++++++++ .../mrpt/nav/reactive/TCandidateMovementPTG.h | 2 +- .../reactive/CAbstractPTGBasedReactive.cpp | 20 ------------------ .../navigation-ptgs/reactive2d_config.ini | 1 + .../navigation-ptgs/reactive3d_config.ini | 1 + .../reactivenav-app-config.ini | 2 ++ 6 files changed, 26 insertions(+), 21 deletions(-) diff --git a/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h b/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h index 8513173172..e6bfeaef92 100644 --- a/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h +++ b/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h @@ -44,6 +44,27 @@ namespace mrpt * Publications: * - See derived classes for papers on each specific method. * + * Available "variables" or "score names" for each motion candidate (these can be used in runtime-compiled expressions + * in the configuration files of motion deciders): + * + * - `ptg_idx`: PTG index (0-based) + * - `ref_dist`: PTG ref distance [m] + * - `target_dir`: Angle of target in TP-Space [rad] + * - `target_k`: Same as target_dir but in discrete path 0-based indices. + * - `target_d_norm`: Normalized target distance. Can be >1 if distance is larger than ref_distance. + * - `move_k`: Motion candidate path 0-based index. + * - `is_PTG_cont`: 1 (is "NOP" motion command), 0 otherwise + * - `num_paths`: Number of paths in the PTG + * - `WS_target_x`, `WS_target_y`: Target coordinates in realworld [m] + * - `robpose_x`, `robpose_y`, `robpose_phi`: Robot pose ([m] and [rad]) at the "end of trajectory": at collision or at target distance. + * - `ptg_priority`: Product of PTG getScorePriority() times PTG evalPathRelativePriority() + * - `collision_free_distance`: Normalized [0,1] collision-free distance in selected path. + * - `dist_eucl_final`: Euclidean distance (in the real-world WordSpace) between "end of trajectory" and target. + * - `hysteresis`: Measure of similarity with previous command [0,1] + * - `clearance`: Clearance (larger means larger distances to obstacles) for the path from "current pose" up to "end of trajectory". + * - `eta`: Estimated Time of Arrival at "end of trajectory". + * - `original_col_free_dist`: Only for "NOP motions", the collision-free distance when the motion command was originally issued. + * * \sa CReactiveNavigationSystem, CReactiveNavigationSystem3D * \ingroup nav_reactive */ diff --git a/libs/nav/include/mrpt/nav/reactive/TCandidateMovementPTG.h b/libs/nav/include/mrpt/nav/reactive/TCandidateMovementPTG.h index 91aa5bff65..05640d434b 100644 --- a/libs/nav/include/mrpt/nav/reactive/TCandidateMovementPTG.h +++ b/libs/nav/include/mrpt/nav/reactive/TCandidateMovementPTG.h @@ -29,7 +29,7 @@ namespace mrpt double starting_robot_dir, starting_robot_dist; //!< Default to 0, they can be used to reflect a robot starting at a position not at (0,0). Used in "PTG continuation" /** List of properties. May vary for different user implementations of scores and/or different multi-objective optimizers. - * Common entries: `eval_prio`, `evaluation` + * See list of available variable names in docs for mrpt::nav::CAbstractPTGBasedReactive */ mrpt::utils::TParameters props; diff --git a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp index 381b057f72..45868ddb1d 100644 --- a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp +++ b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp @@ -858,28 +858,8 @@ void CAbstractPTGBasedReactive::calc_move_candidate_scores( } } - //// Factor 2: Distance in sectors: - //// ------------------------------------------- - ////int dif = std::abs(TargetSector - kDirection); - ////if ( dif > int(nSectors/2)) dif = nSectors - dif; - //m_expr_var_k = move_k; - //m_expr_var_k_target = target_k; - //m_expr_var_num_paths = in_TPObstacles.size(); - - //// Was: eval_factors[SCOREIDX_TPS_DIRECTION] = exp(-std::abs( dif / (nSectors/10.0))); - //eval_factors[SCOREIDX_TPS_DIRECTION] = PIMPL_GET_CONSTREF(exprtk::expression, m_expr_score2_formula).value(); - - // Factor 3: Angle between the robot at the end of the chosen trajectory and the target - // ------------------------------------------------------------------------------------- - //double t_ang = atan2( WS_Target.y - pose.y, WS_Target.x - pose.x ); - //t_ang -= pose.phi; - //mrpt::math::wrapToPiInPlace(t_ang); - - //eval_factors[SCOREIDX_ORIENTATION_AT_END] = exp(-square( t_ang / (0.5*M_PI)) ); - // Factor4: Decrease in euclidean distance between (x,y) and the target: // Moving away of the target is negatively valued. - // --------------------------------------------------------------------------- cm.props["dist_eucl_final"] = std::hypot(WS_Target.x- pose.x, WS_Target.y- pose.y); // Factor5: Hysteresis: diff --git a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini index 8ce529e5cf..c5d3d7cbfd 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini @@ -97,6 +97,7 @@ TARGET_ATTRACTIVE_FORCE = 20.000000 // Dime # Each one defines 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/structmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive_1_1_t_navigation_params_p_t_g.html score1_name = target_distance score1_formula = target_d_norm diff --git a/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini index b7e0363e28..b921717719 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini @@ -97,6 +97,7 @@ TARGET_ATTRACTIVE_FORCE = 20.000000 // Dime # Each one defines 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/structmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive_1_1_t_navigation_params_p_t_g.html score1_name = target_distance score1_formula = target_d_norm diff --git a/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini b/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini index 9658e0b641..1c2886ec88 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini @@ -106,6 +106,7 @@ TARGET_ATTRACTIVE_FORCE = 20.000000 // Dime # Each one defines 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/structmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive_1_1_t_navigation_params_p_t_g.html score1_name = target_distance score1_formula = target_d_norm @@ -301,6 +302,7 @@ RobotModel_circular_shape_radius = 0.36 # Each one defines 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/structmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive_1_1_t_navigation_params_p_t_g.html score1_name = target_distance score1_formula = target_d_norm