/*---------------------------------------------------------------
Navigate
---------------------------------------------------------------*/
void CHolonomicND::navigate(
const mrpt::math::TPoint2D &target,
const std::vector<double> &obstacles,
double maxRobotSpeed,
double &desiredDirection,
double &desiredSpeed,
CHolonomicLogFileRecordPtr &logRecord,
const double max_obstacle_dist)
{
TGapArray gaps;
TSituations situation;
unsigned int selectedSector;
double riskEvaluation;
CLogFileRecord_NDPtr log;
double evaluation;
// Create a log record for returning data.
if (!logRecord.present())
{
log = CLogFileRecord_ND::Create();
logRecord = log;
}
// Search gaps:
gaps.clear();
gapsEstimator( obstacles, target, gaps);
// Select best gap:
searchBestGap( obstacles,
1.0,
gaps,
target,
selectedSector,
evaluation,
situation,
riskEvaluation,
log);
if (situation == SITUATION_NO_WAY_FOUND)
{
// No way found!
desiredDirection = 0;
desiredSpeed = 0;
}
else
{
// A valid movement:
desiredDirection = (double)(M_PI*(-1 + 2*(0.5f+selectedSector)/((double)obstacles.size())));
// Speed control: Reduction factors
// ---------------------------------------------
const double targetNearnessFactor = std::min( 1.0, target.norm()/(options.TARGET_SLOW_APPROACHING_DISTANCE));
const double riskFactor = std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE );
desiredSpeed = maxRobotSpeed * std::min(riskFactor,targetNearnessFactor);
}
m_last_selected_sector = selectedSector;
// LOG --------------------------
if (log)
{
// gaps:
{
int i,n = gaps.size();
log->gaps_ini.resize(n);
log->gaps_end.resize(n);
for (i=0;i<n;i++)
{
log->gaps_ini[i] = gaps[i].ini;
log->gaps_end[i] = gaps[i].end;
}
}
// Selection:
log->selectedSector = selectedSector;
log->evaluation = evaluation;
log->situation = situation;
log->riskEvaluation = riskEvaluation;
}
}
/*---------------------------------------------------------------
Evaluate each gap
---------------------------------------------------------------*/
void CHolonomicND::evaluateGaps(
const std::vector<double> &obstacles,
const double maxObsRange,
const TGapArray &gaps,
const unsigned int target_sector,
const float target_dist,
std::vector<double> &out_gaps_evaluation )
{
out_gaps_evaluation.resize( gaps.size());
double targetAng = M_PI*(-1 + 2*(0.5+target_sector)/double(obstacles.size()));
double target_x = target_dist*cos(targetAng);
double target_y = target_dist*sin(targetAng);
for (unsigned int i=0;i<gaps.size();i++)
{
// Short cut:
const TGap *gap = &gaps[i];
const float d = min3(
obstacles[ gap->representative_sector ],
maxObsRange,
0.95*target_dist );
// The TP-Space representative coordinates for this gap:
const double phi = M_PI*(-1 + 2*(0.5+gap->representative_sector)/double(obstacles.size()));
const double x = d*cos(phi);
const double y = d*sin(phi);
// Factor #1: Maximum reachable distance with this PTG:
// -----------------------------------------------------
// It computes the average free distance of the gap:
float meanDist = 0.f;
for (unsigned int j=gap->ini;j<=gap->end;j++)
meanDist+= obstacles[j];
meanDist/= ( gap->end - gap->ini + 1);
double factor1;
if (mrpt::utils::abs_diff(gap->representative_sector,target_sector)<=1 && target_dist<1)
factor1 = std::min(target_dist,meanDist) / target_dist;
else factor1 = meanDist;
// Factor #2: Distance to target in "sectors"
// -------------------------------------------
unsigned int dif = mrpt::utils::abs_diff(target_sector, gap->representative_sector );
// Handle the -PI,PI circular topology:
if (dif> 0.5*obstacles.size())
dif = obstacles.size() - dif;
const double factor2= exp(-square( dif / (obstacles.size()*0.25))) ;
// Factor #3: Punish paths that take us far away wrt the target: **** I don't understand it *********
// -----------------------------------------------------
double closestX,closestY;
double dist_eucl = math::minimumDistanceFromPointToSegment(
target_x, target_y, // Point
0,0, x,y, // Segment
closestX,closestY // Out
);
const float factor3 = ( maxObsRange - std::min(maxObsRange ,dist_eucl) ) / maxObsRange;
// Factor #4: Stabilizing factor (hysteresis) to avoid quick switch among very similar paths:
// ------------------------------------------------------------------------------------------
double factor_AntiCab;
if (m_last_selected_sector != std::numeric_limits<unsigned int>::max() )
{
unsigned int dist = mrpt::utils::abs_diff(m_last_selected_sector, gap->representative_sector);
if (dist > unsigned(0.1*obstacles.size()))
factor_AntiCab = 0.0;
else
factor_AntiCab = 1.0;
}
else
{
factor_AntiCab = 0;
}
ASSERT_(options.factorWeights.size()==4);
if ( obstacles[gap->representative_sector] < options.TOO_CLOSE_OBSTACLE ) // Too close to obstacles
out_gaps_evaluation[i] = 0;
else out_gaps_evaluation[i] = (
options.factorWeights[0] * factor1 +
options.factorWeights[1] * factor2 +
options.factorWeights[2] * factor3 +
options.factorWeights[3] * factor_AntiCab ) / (math::sum(options.factorWeights)) ;
} // for each gap
}
/*---------------------------------------------------------------
Search the best gap.
---------------------------------------------------------------*/
void CHolonomicND::searchBestGap(
const std::vector<double> & obstacles,
const double maxObsRange,
const TGapArray & in_gaps,
const mrpt::math::TPoint2D & target,
unsigned int & out_selDirection,
double & out_selEvaluation,
TSituations & out_situation,
double & out_riskEvaluation,
CLogFileRecord_NDPtr log)
{
// For evaluating the "risk":
unsigned int min_risk_eval_sector = 0;
unsigned int max_risk_eval_sector = obstacles.size()-1;
const unsigned int target_sector = direction2sector(atan2(target.y,target.x),obstacles.size());
const double target_dist = std::max(0.01,target.norm());
// (Risk is evaluated at the end, for all the situations)
// D1 : Straight path?
// --------------------------------------------------------
const int freeSectorsNearTarget = ceil(0.02*obstacles.size());
bool theyAreFree = true, caseD1 = false;
if (target_sector>static_cast<unsigned int>(freeSectorsNearTarget) &&
target_sector<static_cast<unsigned int>(obstacles.size()-freeSectorsNearTarget) )
{
const double min_free_dist = std::min(1.05*target_dist, 0.95*maxObsRange);
for (int j=-freeSectorsNearTarget;theyAreFree && j<=freeSectorsNearTarget;j++)
if (obstacles[ (int(target_sector) + j) % obstacles.size()]<min_free_dist)
theyAreFree = false;
caseD1 = theyAreFree;
}
if (caseD1)
{
// S1: Move straight towards target:
out_selDirection = target_sector;
// In case of several paths, the shortest:
out_selEvaluation = 1.0 + std::max( 0.0, (maxObsRange - target_dist) / maxObsRange );
out_situation = SITUATION_TARGET_DIRECTLY;
}
else
{
// Evaluate all gaps (if any):
std::vector<double> gaps_evaluation;
int selected_gap =-1;
double selected_gap_eval = -100;
evaluateGaps(
obstacles,
maxObsRange,
in_gaps,
target_sector,
target_dist,
gaps_evaluation );
if (log) log->gaps_eval = gaps_evaluation;
// D2: is there any gap "beyond" the target (and not too far away)? (Not used)
// ----------------------------------------------------------------
//unsigned int dist;
//for ( unsigned int i=0;i<in_gaps.size();i++ )
//{
// dist = mrpt::utils::abs_diff(target_sector, in_gaps[i].representative_sector );
// if (dist > 0.5*obstacles.size())
// dist = obstacles.size() - dist;
//
// if ( in_gaps[i].maxDistance >= target_dist && dist <= (int)floor(options.MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size()) )
//
// if ( gaps_evaluation[i]>selected_gap_eval )
// {
// selected_gap_eval = gaps_evaluation[i];
// selected_gap = i;
// }
//}
// Keep the best gaps (if none was picked up to this point)
if ( selected_gap==-1 )
for ( unsigned int i=0;i<in_gaps.size();i++ )
if ( gaps_evaluation[i]>selected_gap_eval )
{
selected_gap_eval = gaps_evaluation[i];
selected_gap = i;
}
// D3: Wasn't a good enough gap (or there were none)?
// ----------------------------------------------------------
if ( selected_gap_eval <= 0 )
{
// S2: No way found
// ------------------------------------------------------
out_selDirection = 0;
out_selEvaluation = 0.0; // Worst case
out_situation = SITUATION_NO_WAY_FOUND;
}
else
{
// The selected gap:
const TGap & gap = in_gaps[selected_gap];
const unsigned int sectors_to_be_wide = round( options.WIDE_GAP_SIZE_PERCENT * obstacles.size() );
out_selDirection = in_gaps[selected_gap].representative_sector;
out_selEvaluation = selected_gap_eval;
// D4: Is it a WIDE gap?
// -----------------------------------------------------
if ( (gap.end-gap.ini) < sectors_to_be_wide )
{
// S3: Narrow gap
// -------------------------------------------
out_situation = SITUATION_SMALL_GAP;
}
else
{
// S4: Wide gap
// -------------------------------------------
out_situation = SITUATION_WIDE_GAP;
}
// Evaluate the risk only within the gap:
min_risk_eval_sector = gap.ini;
max_risk_eval_sector = gap.end;
}
}
// Evaluate short-term minimum distance to obstacles, in a small interval around the selected direction:
const unsigned int risk_eval_nsectors = round( options.RISK_EVALUATION_SECTORS_PERCENT * obstacles.size() );
const unsigned int sec_ini = std::max(min_risk_eval_sector, risk_eval_nsectors<out_selDirection ? out_selDirection-risk_eval_nsectors : 0 );
const unsigned int sec_fin = std::min(max_risk_eval_sector, out_selDirection + risk_eval_nsectors );
out_riskEvaluation = 0.0;
for (unsigned int i=sec_ini;i<=sec_fin;i++) out_riskEvaluation+= obstacles[ i ];
out_riskEvaluation /= (sec_fin - sec_ini + 1 );
}
/*---------------------------------------------------------------
Navigate
---------------------------------------------------------------*/
void CHolonomicND::navigate(
poses::CPoint2D &target,
vector_double &obstacles,
double maxRobotSpeed,
double &desiredDirection,
double &desiredSpeed,
CHolonomicLogFileRecordPtr &logRecord)
{
TGapArray gaps;
TSituations situation;
int selectedSector;
double riskEvaluation;
CLogFileRecord_NDPtr log;
double evaluation;
// Create a log record for returning data.
if (!logRecord.present())
{
log = CLogFileRecord_ND::Create();
logRecord = log;
}
// Search gaps:
gaps.clear();
gapsEstimator( obstacles,
target,
gaps );
// Select best gap:
searchBestGap( obstacles,
1.0f,
gaps,
target,
selectedSector,
evaluation,
situation,
riskEvaluation,
log);
if (situation == SITUATION_NO_WAY_FOUND)
{
// No way found!
desiredDirection = 0;
desiredSpeed = 0;
}
else
{
// A valid movement:
desiredDirection = (double)(M_PI*(-1 + 2*(0.5f+selectedSector)/((double)obstacles.size())));
// Speed control: Reduction factors
// ---------------------------------------------
double targetNearnessFactor = max(0.20, min(1.0, 1.0-exp(-(target.norm()+0.01)/TARGET_SLOW_APPROACHING_DISTANCE)));
//printf(" TARGET NEARNESS = %f\n",targetNearnessFactor);
double riskFactor = min(1.0, riskEvaluation / RISK_EVALUATION_DISTANCE );
desiredSpeed = maxRobotSpeed * min(riskFactor,targetNearnessFactor);
}
last_selected_sector = selectedSector;
// LOG --------------------------
if (log)
{
// gaps:
if (situation != SITUATION_TARGET_DIRECTLY )
{
int i,n = gaps.size();
log->gaps_ini.resize(n);
log->gaps_end.resize(n);
for (i=0;i<n;i++)
{
log->gaps_ini[i] = gaps[i].ini;
log->gaps_end[i] = gaps[i].end;
}
}
// Selection:
log->selectedSector = selectedSector;
log->evaluation = evaluation;
log->situation = situation;
log->riskEvaluation = riskEvaluation;
}
}
/*---------------------------------------------------------------
Evaluate each gap
---------------------------------------------------------------*/
void CHolonomicND::evaluateGaps(
const vector_double &obstacles,
const double maxObsRange,
const TGapArray &gaps,
const int TargetSector,
const double TargetDist,
vector_double &out_gaps_evaluation )
{
out_gaps_evaluation.resize( gaps.size());
double targetAng = M_PI*(-1 + 2*(0.5+TargetSector)/double(obstacles.size()));
double target_x = TargetDist*cos(targetAng);
double target_y = TargetDist*sin(targetAng);
for (unsigned int i=0;i<gaps.size();i++)
{
// Para referenciarlo mas facilmente:
const TGap *gap = &gaps[i];
double d;
d = min( obstacles[ gap->representative_sector ],
min( maxObsRange, 0.95f*TargetDist) );
// Las coordenadas (en el TP-Space) representativas del gap:
double phi = M_PI*(-1 + 2*(0.5+gap->representative_sector)/double(obstacles.size()));
double x = d*cos(phi);
double y = d*sin(phi);
// Factor 1: Distancia hasta donde llego por esta GPT:
// -----------------------------------------------------
double factor1;
/* if (gap->representative_sector == TargetSector )
factor1 = min(TargetDist,obstacles[gap->representative_sector]) / TargetDist;
else
{
if (TargetDist>1)
factor1 = obstacles[gap->representative_sector] / TargetDist;
else factor1 = obstacles[gap->representative_sector];
}
*/
// Calcular la distancia media a donde llego por este gap:
double meanDist = 0;
for (int j=gap->ini;j<=gap->end;j++)
meanDist+= obstacles[j];
meanDist/= ( gap->end - gap->ini + 1);
if (abs(gap->representative_sector-TargetSector)<=1 && TargetDist<1)
factor1 = min(TargetDist,meanDist) / TargetDist;
else factor1 = meanDist;
// Factor 2: Distancia en sectores:
// -------------------------------------------
double dif = fabs(((double)( TargetSector - gap->representative_sector )));
// if (dif> (0.5f*obstacles.size()) ) dif = obstacles.size() - dif;
// Solo si NO estan el target y el gap atravesando el alfa = "-pi" o "pi"
if (dif> (0.5f*obstacles.size()) && (TargetSector-0.5f*obstacles.size())*(gap->representative_sector-0.5f*obstacles.size())<0 )
dif = obstacles.size() - dif;
double factor2= exp(-square( dif / (obstacles.size()/4))) ;
// Factor3: Para evitar cabeceos entre 2 o mas caminos que sean casi iguales:
// -------------------------------------------
double dist = (double)(abs(last_selected_sector - gap->representative_sector));
//
if (dist> (0.5f*obstacles.size()) ) dist = obstacles.size() - dist;
double factor_AntiCab;
if (last_selected_sector==-1)
factor_AntiCab = 0;
else factor_AntiCab = (dist > 0.10f*obstacles.size()) ? 0.0f:1.0f;
// Factor3: Minima distancia entre el segmento y el target:
// Se valora negativamente el alejarse del target
// -----------------------------------------------------
double closestX,closestY;
double dist_eucl = math::minimumDistanceFromPointToSegment(
target_x,
target_y,
0,0,
x,y,
closestX,closestY);
double factor3= ( maxObsRange - min(maxObsRange ,dist_eucl) ) / maxObsRange;
ASSERT_(factorWeights.size()==4);
if ( obstacles[gap->representative_sector] < TOO_CLOSE_OBSTACLE ) // Too close to obstacles
out_gaps_evaluation[i] = 0;
else out_gaps_evaluation[i] = (
factorWeights[0] * factor1 +
factorWeights[1] * factor2 +
factorWeights[2] * factor3 +
factorWeights[3] * factor_AntiCab ) / (math::sum(factorWeights)) ;
} // for each gap
}
/*---------------------------------------------------------------
Search the best gap.
---------------------------------------------------------------*/
void CHolonomicND::searchBestGap(
vector_double &obstacles,
double maxObsRange,
TGapArray &in_gaps,
poses::CPoint2D &target,
int &out_selDirection,
double &out_selEvaluation,
TSituations &out_situation,
double &out_riskEvaluation,
CLogFileRecord_NDPtr log)
{
// Para evaluar el risk:
unsigned int min_risk_eval_sector = 0;
unsigned int max_risk_eval_sector = obstacles.size()-1;
unsigned int TargetSector = direction2sector(atan2(target.y(),target.x()),obstacles.size());
const double TargetDist = std::max(0.01,target.norm());
// El "risk" se calcula al final para todos los casos.
// D1 : Camino directo?
// --------------------------------------------------------
const int freeSectorsNearTarget = 10; // 3
bool theyAreFree = true, caseD1 = false;
if (TargetSector>(unsigned int)freeSectorsNearTarget &&
TargetSector<(unsigned int)(obstacles.size()-freeSectorsNearTarget) )
{
for (int j=-freeSectorsNearTarget;j<=freeSectorsNearTarget;j++)
if (obstacles[ TargetSector + j ]<0.95*TargetDist)
theyAreFree = false;
caseD1 = theyAreFree;
}
if (caseD1)
{
// S1: Camino libre hacia target:
out_selDirection = TargetSector;
// Si hay mas de una, la que llegue antes
out_selEvaluation = 1.0 + std::max( 0.0, (maxObsRange - TargetDist) / maxObsRange );
out_situation = SITUATION_TARGET_DIRECTLY;
}
else
{
// Evaluar los GAPs (Si no hay ninguno, nada, claro):
vector_double gaps_evaluation;
int selected_gap =-1;
double selected_gap_eval = -100;
evaluateGaps(
obstacles,
maxObsRange,
in_gaps,
TargetSector,
TargetDist,
gaps_evaluation );
if (log) log->gaps_eval = gaps_evaluation;
// D2: Hay algun gap que pase por detras del target?
// ( y no este demasiado lejos):
// -------------------------------------------------
for ( unsigned int i=0;i<in_gaps.size();i++ )
if ( in_gaps[i].maxDistance >= TargetDist &&
abs((int)(in_gaps[i].representative_sector-(int)TargetSector)) <= (int)floor(MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size()) )
if ( gaps_evaluation[i]>selected_gap_eval )
{
selected_gap_eval = gaps_evaluation[i];
selected_gap = i;
}
// Coger el mejor GAP:
// (Esto ya solo si no se ha cogido antes)
if ( selected_gap==-1 )
for ( unsigned int i=0;i<in_gaps.size();i++ )
if ( gaps_evaluation[i]>selected_gap_eval )
{
selected_gap_eval = gaps_evaluation[i];
selected_gap = i;
}
// D3: No es suficientemente bueno? ( o no habia ninguno?)
// ------------------------------------------------------------
if ( selected_gap_eval <= 0 )
{
// S2: No way found
// ------------------------------------------------------
out_selDirection = 0;
out_selEvaluation = 0.0f; // La peor
out_situation = SITUATION_NO_WAY_FOUND;
}
else
{
// El seleccionado:
TGap gap = in_gaps[selected_gap];
int sectors_to_be_wide = round( WIDE_GAP_SIZE_PERCENT * obstacles.size() );
out_selDirection = in_gaps[selected_gap].representative_sector;
out_selEvaluation = selected_gap_eval;
// D4: Es un gap ancho?
// -----------------------------------------------------
if ( (gap.end-gap.ini) < sectors_to_be_wide )
{
// S3: Small gap
// -------------------------------------------
out_situation = SITUATION_SMALL_GAP;
}
else
{
// S4: Wide gap
// -------------------------------------------
out_situation = SITUATION_WIDE_GAP;
}
// Que el risk no salga del gap:
min_risk_eval_sector = gap.ini;
max_risk_eval_sector = gap.end;
}
}
// Calcular minima distancia a obstaculos a corto plazo, en
// un intervalo de sectores en torno al sector elegido:
int ancho_sectores = round( RISK_EVALUATION_SECTORS_PERCENT * obstacles.size() );
int sec_ini = max((int)min_risk_eval_sector, out_selDirection - ancho_sectores );
int sec_fin = min((int)max_risk_eval_sector, out_selDirection + ancho_sectores );
out_riskEvaluation = 0.0;
for (int i=sec_ini;i<=sec_fin;i++) out_riskEvaluation+= obstacles[ i ];
out_riskEvaluation /= (sec_fin - sec_ini + 1 );
}
/*---------------------------------------------------------------
Navigate
---------------------------------------------------------------*/
void CHolonomicND::navigate(const NavInput& ni, NavOutput& no)
{
const auto ptg = getAssociatedPTG();
const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
TGapArray gaps;
TSituations situation;
unsigned int selectedSector;
double riskEvaluation;
double evaluation;
// Create a log record for returning data.
CLogFileRecord_ND::Ptr log = mrpt::make_aligned_shared<CLogFileRecord_ND>();
no.logRecord = log;
// Search gaps:
gaps.clear();
ASSERT_(!ni.targets.empty());
const auto trg = *ni.targets.rbegin();
gapsEstimator(ni.obstacles, trg, gaps);
// Select best gap:
searchBestGap(
ni.obstacles, 1.0 /* max obs range*/, gaps, trg, selectedSector,
evaluation, situation, riskEvaluation, *log);
if (situation == SITUATION_NO_WAY_FOUND)
{
// No way found!
no.desiredDirection = 0;
no.desiredSpeed = 0;
}
else
{
// A valid movement:
no.desiredDirection = CParameterizedTrajectoryGenerator::index2alpha(
selectedSector, ni.obstacles.size());
// Speed control: Reduction factors
// ---------------------------------------------
const double targetNearnessFactor =
m_enableApproachTargetSlowDown
? std::min(
1.0,
trg.norm() / (options.TARGET_SLOW_APPROACHING_DISTANCE /
ptg_ref_dist))
: 1.0;
const double riskFactor =
std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE);
no.desiredSpeed =
ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
}
m_last_selected_sector = selectedSector;
// LOG --------------------------
if (log)
{
// gaps:
{
int i, n = gaps.size();
log->gaps_ini.resize(n);
log->gaps_end.resize(n);
for (i = 0; i < n; i++)
{
log->gaps_ini[i] = gaps[i].ini;
log->gaps_end[i] = gaps[i].end;
}
}
// Selection:
log->selectedSector = selectedSector;
log->evaluation = evaluation;
log->situation = situation;
log->riskEvaluation = riskEvaluation;
}
}