/*---------------------------------------------------------------
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;
}
}
/*---------------------------------------------------------------
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;
}
}
