/*---------------------------------------------------------------
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;
}
}
/*---------------------------------------------------------------
Find gaps in the obtacles (Beta version)
---------------------------------------------------------------*/
void CHolonomicND::gapsEstimator(
const std::vector<double> & obstacles,
const mrpt::math::TPoint2D & target,
TGapArray & gaps_out)
{
const size_t n = obstacles.size();
ASSERT_(n>2);
// ================ Parameters ================
const int GAPS_MIN_WIDTH = ceil(n*0.01); // was: 3
const double GAPS_MIN_DEPTH_CONSIDERED = 0.6;
const double GAPS_MAX_RELATIVE_DEPTH = 0.5;
// ============================================
// Find the maximum distances to obstacles:
// ----------------------------------------------------------
float overall_max_dist = std::numeric_limits<float>::min(), overall_min_dist = std::numeric_limits<float>::max();
for (size_t i=1;i<(n-1);i++)
{
mrpt::utils::keep_max(overall_max_dist, obstacles[i]);
mrpt::utils::keep_min(overall_min_dist, obstacles[i]);
}
double max_depth = overall_max_dist - overall_min_dist;
// Build list of "GAPS":
// --------------------------------------------------------
TGapArray gaps_temp;
gaps_temp.reserve( 150 );
for (double threshold_ratio = 0.95;threshold_ratio>=0.05;threshold_ratio-=0.05)
{
const double dist_threshold = threshold_ratio* overall_max_dist + (1.0f-threshold_ratio)*min(target.norm(), GAPS_MIN_DEPTH_CONSIDERED);
bool is_inside = false;
size_t sec_ini=0, sec_end=0;
double maxDist=0.;
for (size_t i=0;i<n;i++)
{
if ( !is_inside && ( obstacles[i]>=dist_threshold) ) //A gap begins
{
sec_ini = i;
maxDist = obstacles[i];
is_inside = true;
}
else if (is_inside && (i==(n-1) || obstacles[i]<dist_threshold )) //A gap ends
{
if (obstacles[i]<dist_threshold)
sec_end = i-1;
else
sec_end = i;
is_inside = false;
if ( (sec_end-sec_ini) >= (size_t)GAPS_MIN_WIDTH )
{
// Add new gap:
gaps_temp.resize( gaps_temp.size() + 1 );
TGap & newGap = *gaps_temp.rbegin();
newGap.ini = sec_ini;
newGap.end = sec_end;
newGap.minDistance = min( obstacles[sec_ini], obstacles[sec_end] );
newGap.maxDistance = maxDist;
}
}
if (is_inside)
maxDist = std::max( maxDist, obstacles[i] );
}
}
//Start to filter the gap list
//--------------------------------------------------------------
const size_t nTempGaps = gaps_temp.size();
std::vector<bool> delete_gaps;
delete_gaps.assign( nTempGaps, false);
// First, remove redundant gaps
for (size_t i=0;i<nTempGaps;i++)
{
if (delete_gaps[i] == 1)
continue;
for (size_t j=i+1;j<nTempGaps;j++)
{
if (gaps_temp[i].ini == gaps_temp[j].ini || gaps_temp[i].end == gaps_temp[j].end)
delete_gaps[j] = 1;
}
}
// Remove gaps with a big depth
for (size_t i=0;i<nTempGaps;i++)
{
if (delete_gaps[i] == 1)
continue;
if ((gaps_temp[i].maxDistance - gaps_temp[i].minDistance) > max_depth*GAPS_MAX_RELATIVE_DEPTH)
delete_gaps[i] = 1;
}
//Delete gaps which contain more than one other gaps
for (size_t i=0;i<nTempGaps;i++)
{
if (delete_gaps[i])
continue;
unsigned int inner_gap_count = 0;
for (unsigned int j=0;j<nTempGaps;j++)
{
if (i==j || delete_gaps[j])
continue;
// j is inside of i?
if (gaps_temp[j].ini >= gaps_temp[i].ini && gaps_temp[j].end <= gaps_temp[i].end )
if (++inner_gap_count>1)
{
delete_gaps[i] = 1;
break;
}
}
}
//Delete gaps included in other gaps
for (size_t i=0;i<nTempGaps;i++)
{
if (delete_gaps[i])
continue;
for (unsigned int j=0;j<nTempGaps;j++)
{
if (i==j || delete_gaps[j])
continue;
if (gaps_temp[i].ini <= gaps_temp[j].ini && gaps_temp[i].end >= gaps_temp[j].end)
delete_gaps[j] = 1;
}
}
// Copy as result only those gaps not marked for deletion:
// --------------------------------------------------------
gaps_out.clear();
gaps_out.reserve( nTempGaps/2 );
for (size_t i=0;i<nTempGaps;i++)
{
if (delete_gaps[i]) continue;
// Compute the representative direction ("sector") for this gap:
calcRepresentativeSectorForGap( gaps_temp[i], target, obstacles);
gaps_out.push_back( gaps_temp[i] );
}
}
/*---------------------------------------------------------------
Find gaps in the obtacles.
---------------------------------------------------------------*/
void CHolonomicND::gapsEstimator(
vector_double &obstacles,
poses::CPoint2D &target,
TGapArray &gaps_out )
{
unsigned int i,n;
int nMaximos=0;
double MaximoAbsoluto = -100;
double MinimoAbsoluto = 100;
vector_int MaximoIdx;
vector_double MaximoValor;
// Hacer una lista con los maximos de las distancias a obs:
// ----------------------------------------------------------
MaximoIdx.resize(obstacles.size());
MaximoValor.resize(obstacles.size());
n = obstacles.size();
for (i=1;i<(n-1);i++)
{
// Actualizar max. y min. absolutos:
MaximoAbsoluto= max( MaximoAbsoluto, obstacles[i] );
MinimoAbsoluto= min( MinimoAbsoluto, obstacles[i] );
// Buscar maximos locales:
if ( ( obstacles[i] >= obstacles[i+1] &&
obstacles[i] > obstacles[i-1] ) ||
( obstacles[i] > obstacles[i+1] &&
obstacles[i] >= obstacles[i-1] ) )
{
MaximoIdx[nMaximos] = i;
MaximoValor[nMaximos++] = obstacles[i];
}
}
// Crear GAPS:
// --------------------------------------------------------
TGapArray gaps_temp;
gaps_temp.reserve( 150 );
for (double factorUmbral = 0.975f;factorUmbral>=0.04f;factorUmbral-=0.05f)
{
double umbral = factorUmbral* MaximoAbsoluto + (1.0f-factorUmbral)*MinimoAbsoluto;
bool dentro = false;
int sec_ini=0, sec_end;
double maxDist=0;
for (i=0;i<n;i++)
{
if ( !dentro && (!i || obstacles[i]>=umbral) )
{
sec_ini = i;
maxDist = obstacles[i];
dentro = true;
}
else if (dentro && (i==(n-1) || obstacles[i]<umbral ))
{
sec_end = i;
dentro = false;
if ( (sec_end-sec_ini) > 2 )
{
// Add new gap:
TGap newGap;
newGap.ini = sec_ini;
newGap.end = sec_end;
newGap.entranceDistance = min( obstacles[sec_ini], obstacles[sec_end] );
newGap.maxDistance = maxDist;
gaps_temp.push_back(newGap);
}
}
if (dentro) maxDist = max( maxDist, obstacles[i] );
}
}
// Proceso de eliminacion de huecos redundantes:
// -------------------------------------------------------------
std::vector<bool> borrar_gap;
borrar_gap.resize( gaps_temp.size() );
for (i=0;i<gaps_temp.size();i++)
borrar_gap[i] = false;
// Eliminar huecos con muy poca profundidad si estan dentro de otros:
double maxProfundidad = 0;
for (i=0;i<gaps_temp.size();i++)
{
double profundidad =
gaps_temp[i].maxDistance -
gaps_temp[i].entranceDistance;
maxProfundidad = max(maxProfundidad, profundidad);
}
for (i=0;i<gaps_temp.size();i++)
{
double profundidad =
gaps_temp[i].maxDistance -
gaps_temp[i].entranceDistance;
if ( profundidad< maxProfundidad / 10.0f )
borrar_gap[i]=true;
}
// Si es muy estrecho, pero hay uno casi igual pero UN POCO mas grande,
// borrar el estrecho:
for (i=0;i<gaps_temp.size();i++)
{
int ini_i = gaps_temp[i].ini;
int fin_i = gaps_temp[i].end;
int ancho_i = fin_i - ini_i;
if ( !borrar_gap[i] )
{
for (unsigned int j=0;j<gaps_temp.size() && !borrar_gap[i];j++)
{
if (i!=j)
{
int ini_j = gaps_temp[j].ini;
int fin_j = gaps_temp[j].end;
int ancho_j = fin_j - ini_j;
// j dentro de i y UN POCO mas grande nada mas:
if ( !borrar_gap[j] &&
ini_j>=ini_i &&
fin_j<=fin_i &&
ancho_i < (0.05f*n) &&
ancho_j < (0.25f*n)
)
borrar_gap[i] = true;
}
}
}
}
// Si dentro tiene mas de 1, borrarlo:
for (i=0;i<gaps_temp.size();i++)
{
int ini_i = gaps_temp[i].ini;
int fin_i = gaps_temp[i].end;
int nDentro = 0;
if ( !borrar_gap[i] )
{
for (unsigned int j=0;j<gaps_temp.size();j++)
{
if (i!=j)
{
int ini_j = gaps_temp[j].ini;
int fin_j = gaps_temp[j].end;
// j dentro de i:
if ( !borrar_gap[j] &&
ini_j>=ini_i &&
fin_j<=fin_i ) nDentro++;
}
}
if (nDentro>1) borrar_gap[i] = true;
}
}
// Uno dentro de otro y practicamente a la misma altura: Eliminarlo tambien:
for (i=0;i<gaps_temp.size();i++)
{
if (!borrar_gap[i])
{
double ent_i = gaps_temp[i].entranceDistance;
int ini_i = gaps_temp[i].ini;
int fin_i = gaps_temp[i].end;
double MIN_GAPS_ENTR_DIST = (MaximoAbsoluto-MinimoAbsoluto)/10.0f;
for (unsigned int j=0;j<gaps_temp.size() && !borrar_gap[i];j++)
if (i!=j)
{
double ent_j = gaps_temp[j].entranceDistance;
int ini_j = gaps_temp[j].ini;
int fin_j = gaps_temp[j].end;
// j dentro de i y casi misma "altura":
if ( !borrar_gap[j] &&
!borrar_gap[i] &&
ini_j>=ini_i &&
fin_j<=fin_i &&
fabs(ent_i-ent_j)< MIN_GAPS_ENTR_DIST )
borrar_gap[i]=true;
}
}
}
// Copiar solo huecos no marcados para borrar:
// ---------------------------------------------------
gaps_out.clear();
gaps_out.reserve(15);
for (i=0;i<gaps_temp.size();i++)
if ( !borrar_gap[i] )
{
// Calcular direccion representativa:
calcRepresentativeSectorForGap( gaps_temp[i], target, obstacles);
gaps_out.push_back( gaps_temp[i] );
}
}
/*---------------------------------------------------------------
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;
}
}
/*---------------------------------------------------------------
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;
}
}