Permalink
Browse files

handle simple direct-to-target motion in FullEval class

  • Loading branch information...
jlblancoc committed Mar 15, 2017
1 parent 3e3c86b commit 7cdd6d9518dee007ad7c24f99034af85a87f5b10
@@ -89,7 +89,7 @@ namespace mrpt
unsigned int direction2sector(const double a, const unsigned int N);
mrpt::math::CMatrixD m_dirs_scores; //!< Individual scores for each direction: (i,j), i (row) are directions, j (cols) are scores. Not all directions may have evaluations, in which case a "-1" value will be found.
virtual void postProcessDirectionEvaluations(std::vector<double> &dir_evals); // If desired, override in a derived class to manipulate the final evaluations of each directions
virtual void postProcessDirectionEvaluations(std::vector<double> &dir_evals, const NavInput & ni); // If desired, override in a derived class to manipulate the final evaluations of each directions
}; // end of CHolonomicFullEval
DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CHolonomicFullEval, CAbstractHolonomicReactiveMethod, NAV_IMPEXP )
@@ -258,7 +258,7 @@ void CHolonomicFullEval::navigate(const NavInput & ni, NavOutput &no)
// Give a chance for a derived class to manipulate the final evaluations:
auto & dirs_eval = *phase_scores.rbegin();
postProcessDirectionEvaluations(dirs_eval);
postProcessDirectionEvaluations(dirs_eval, ni);
// Recalculate the threshold just in case the postProcess function above changed things:
{
@@ -276,15 +276,7 @@ void CHolonomicFullEval::navigate(const NavInput & ni, NavOutput &no)
const unsigned int INVALID_K = std::numeric_limits<unsigned int>::max();
unsigned int best_k = INVALID_K;
double best_eval = .0;
#if 0
// Individual direction search:
for (unsigned int i=0;i<nDirs;i++) {
if (dirs_eval[i]>best_eval) {
best_eval = dirs_eval[i];
best_k = i;
}
}
#else
// Of those directions above "last_phase_threshold", keep the GAP with the largest maximum value within;
// then pick the MIDDLE point as the final selection.
std::vector<TGap> gaps;
@@ -376,7 +368,23 @@ void CHolonomicFullEval::navigate(const NavInput & ni, NavOutput &no)
best_k = mrpt::utils::round(0.5*(best_gap.k_to + best_gap.k_from));
}
#endif
// Alternative, simpler method to decide motion:
// If target can be reached without collision *and* with a minimum of clearance,
// then select that direction, with the score as computed with the regular formulas above
// (even if that score was not the maximum!).
if (target_dist<0.99 && ni.obstacles[target_k]>target_dist*1.01 &&
ni.clearance->getClearance(target_k /*path index*/, std::min(0.99, target_dist*0.95), true /*interpolate path*/)
> options.TOO_CLOSE_OBSTACLE
)
{
best_k = target_k;
best_eval = dirs_eval[target_k];
// Reflect this decision in the phase score plots:
phase_scores[NUM_PHASES - 1][target_k] += 2.0;
}
// Prepare NavigationOutput data:
if (best_eval==.0)
{
// No way found!
@@ -630,7 +638,7 @@ void CHolonomicFullEval::readFromStream(mrpt::utils::CStream &in,int version)
};
}
void CHolonomicFullEval::postProcessDirectionEvaluations(std::vector<double> &dir_evals)
void CHolonomicFullEval::postProcessDirectionEvaluations(std::vector<double> &dir_evals, const NavInput & ni)
{
// Default: do nothing
}

0 comments on commit 7cdd6d9

Please sign in to comment.