/*---------------------------------------------------------------
navigate
---------------------------------------------------------------*/
void CHolonomicVFF::navigate(
const mrpt::math::TPoint2D &target,
const std::vector<float> &obstacles,
double maxRobotSpeed,
double &desiredDirection,
double &desiredSpeed,
CHolonomicLogFileRecordPtr &logRecord)
{
MRPT_UNUSED_PARAM(maxRobotSpeed);
// Create a log record for returning data.
if (!logRecord)
{
logRecord = CLogFileRecord_VFF::Create();
}
// Forces vector:
mrpt::math::TPoint2D resultantForce(0,0),instantaneousForce(0,0);
// Obstacles:
{
const size_t n = obstacles.size();
const double inc_ang = 2*M_PI/n;
double ang = -M_PI + 0.5*inc_ang;
for (size_t i=0;i<n;i++, ang+=inc_ang )
{
// Compute force strength:
//const double mod = exp(- obstacles[i] );
const double mod = std::min(1e6, 1.0/ obstacles[i] );
// Add repulsive force:
instantaneousForce.x = -cos(ang) * mod;
instantaneousForce.y = -sin(ang) * mod;
resultantForce += instantaneousForce;
}
}
const double obstcl_weight = 20.0/obstacles.size();
resultantForce *= obstcl_weight;
const double obstacleNearnessFactor = std::min( 1.0, 6.0/resultantForce.norm());
// Target:
const double ang = atan2( target.y, target.x );
const double mod = options.TARGET_ATTRACTIVE_FORCE;
resultantForce += mrpt::math::TPoint2D(cos(ang) * mod, sin(ang) * mod );
// Result:
desiredDirection = (resultantForce.y==0 && resultantForce.x==0) ?
0 : atan2( resultantForce.y, resultantForce.x );
// Speed control: Reduction factors
// ---------------------------------------------
const double targetNearnessFactor = std::min( 1.0, target.norm()/(options.TARGET_SLOW_APPROACHING_DISTANCE));
//desiredSpeed = maxRobotSpeed * std::min(obstacleNearnessFactor, targetNearnessFactor);
desiredSpeed = std::min(obstacleNearnessFactor, targetNearnessFactor);
}
/*---------------------------------------------------------------
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;
}
}
/*---------------------------------------------------------------
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 );
}
/*---------------------------------------------------------------
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] );
}
}
