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