Exemple #1
0
/*---------------------------------------------------------------
						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);
}
Exemple #2
0
/*---------------------------------------------------------------
						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;
	}
}
Exemple #3
0
/*---------------------------------------------------------------
						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 );
}
Exemple #4
0
/*---------------------------------------------------------------
				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] );
	}

}