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