Exemple #1
0
void TrajectoryFinder::init(StrategyMap* map, Tools::NGrid* grid, const Tools::RPoint &startPoint, int replanInterval,
							int lostReplanInterval, double stopCircleRadius, double diagonalSmoothingMaxDistance, double turnAndMoveMinAngle)
{
	_forcedNextDeplacementType = -1;
	_isArrived = false;
	_isBlocked = false;
	_forceUnblock = true;
	_hasObjective = false;
	_forceStart = false;
	_previousPoint = Tools::RPoint(-1000, -1000);
	_manualAvoidingInProgress = false;
	_pause = false;

	_replanInterval = replanInterval;
	_stopCircle = stopCircleRadius;
	_diagSmoothingMaxDist = diagonalSmoothingMaxDistance;
	_turnAndMoveMinAngle = turnAndMoveMinAngle;

	_lostReplanTimer.setInterval(lostReplanInterval);

	_map = map;
	
	_deplacementType = Tools::TURN_THEN_MOVE;
	_speed = 50;

	_trajectory.clear();
	_currentDestinations.clear();
	_previousPoint = RPoint(-1000, -1000);

	//int Pather
	NGridNode* startNode = grid->getNearestNode(startPoint.toQPointF());
	_pather->init(_map, grid, startNode);
}
Exemple #2
0
Tools::RPoint NSParser::readFixedPoint(Symbol* symbol)
{
	Tools::RPoint point;
	bool xOk = false;
	bool yOk = false;
	if (symbol->type == NON_TERMINAL)
	{
		NonTerminal* nt = static_cast<NonTerminal*>(symbol);
		for(Symbol* child: nt->children)
		{
			switch(child->symbolIndex)
			{
				case SYM_NUM:
				case SYM_FIXED_ANGLE:
				{
					double value = readNum(child);
					if (!xOk)
					{
						point.setX(value);
						xOk = true;
					}
					else if (!yOk)
					{
						point.setY(value);
						yOk = true;
					}
					else
					{
						double angle = readFixedAngleInRadian(child);
						point.setTheta(angle);
					}
					break;
				}
			}
		}
	}

	return point;
}
Exemple #3
0
bool TrajectoryFinder::isArrived(const Tools::RPoint &point) const
{
	if (!_hasObjective || _currentDestinations.isEmpty())
	{
		return false;
	}

	if ((_isPureRotation && _mouvementType == Tools::TOURNE_VERS_XY) || (_mouvementType == Tools::TOURNE_VERS_XY_AR))
	{
		if (fabs(point.angle(_currentDestinations.first()) - point.getTheta()) < 0.2)
			return true;
	}

	if (_isPureRotation &&  fabs(point.diffAngle(_currentDestinations.first())) < 0.2)
		return true;

	if (!_isPureRotation && point.isInCircle(_currentDestinations.first(), 40))
		return true;


	return false;
}
Exemple #4
0
QRectF NSParser::readFixedRect(Symbol* symbol, VariableList& variables)
{
	QRectF r(0,0,1,1);
	Tools::RPoint center;
	bool definedByCenter = false;
	double radius = 0;
	int nbParamRead = 0;
	if (symbol->type == NON_TERMINAL)
	{
		NonTerminal* nt = static_cast<NonTerminal*>(symbol);
		for(Symbol* child: nt->children)
		{
			switch(child->symbolIndex)
			{
				case SYM_NUM:
				{
					double value = readNum(child);
					if (definedByCenter)
						radius = value;
					else if (nbParamRead == 0)
						r.setX(value);
					else if (nbParamRead == 1)
						r.setY(value);
					else if (nbParamRead == 2)
						r.setWidth(value);
					else
						r.setHeight(value);
					++nbParamRead;
					break;
				}
				case SYM_POINT:
				case SYM_FIXED_POINT:
				{
					definedByCenter = true;
					center = readPoint(child);
					break;
				}
				case SYM_VAR:
				{
					QString name = readVar(child);
					if (variables.contains(name))
					{
						DeclaredVariable& var = variables[name];
						if (var.isPoint())
						{
							center = var.toPoint();
							definedByCenter = true;
						}
						else
							addError(NSParsingError::invalidVariableTypeError(name, "point", child));
					}
					else
						addError(NSParsingError::undeclaredVariableError(name, child));


					break;
				}
			}
		}
	}

	if (definedByCenter)
	{
		r.setWidth(radius * 2.0);
		r.setHeight(radius * 2.0);
		r.moveCenter(center.toQPointF());
	}

	return r;
}
Exemple #5
0
bool TrajectoryFinder::pointsEqualsInPather(const Tools::RPoint &p1, const Tools::RPoint &p2) const
{
	return p1.isInCircle(p2, 50); //TODO, make 50 a parameter (minStopRadius)
}