C++ WayPoints::isFinished方法代码示例

bool ElasticBand::update(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData,
                         const CurrentTarget &currentTarget, uint iter)
{
	//qDebug() << __FILE__ << __FUNCTION__ << "road size"<<  road.size();
	if (road.isFinished() == true)
		return false;

	/////////////////////////////////////////////
	//Tags all points in the road ar visible or blocked, depending on laser visibility. Only visible points are processed in this iteration
	/////////////////////////////////////////////
	//checkVisiblePoints(innermodel, road, laserData);

	/////////////////////////////////////////////
	//Check if there is a sudden shortcut to take
	/////////////////////////////////////////////
	//shortCut(innermodel, road, laserData);

	/////////////////////////////////////////////
	//Add points to achieve an homogenoeus chain
	/////////////////////////////////////////////
	addPoints(road, currentTarget);

	/////////////////////////////////////////////
	//Remove point too close to each other
	/////////////////////////////////////////////
	cleanPoints(road);

	/////////////////////////////////////////////
	//Compute the scalar magnitudes
	/////////////////////////////////////////////
	computeForces(innermodel, road, laserData);

	/////////////////////////////////////////////
	//Delete half the tail behind, if greater than 6, to release resources
	/////////////////////////////////////////////
	if (road.getIndexOfClosestPointToRobot() > 6)
	{
		for (auto it = road.begin(); it != road.begin() + (road.getIndexOfCurrentPoint() / 2); ++it)
			road.backList.append(it->pos);
		road.erase(road.begin(), road.begin() + (road.getIndexOfCurrentPoint() / 2));
	}
	return true;
}

bool
SpecificWorker::gotoCommand(InnerModel *innerModel, CurrentTarget &target, TrajectoryState &state, WayPoints &myRoad,
                            RoboCompLaser::TLaserData &lData)
{
	QTime reloj = QTime::currentTime();

	/////////////////////////////////////////////////////
	// check for ending conditions
	//////////////////////////////////////////////////////
	if (myRoad.isFinished() == true)
	{
		controller->stopTheRobot(omnirobot_proxy);
		qDebug() << __FUNCTION__ << "Changing to SETHEADING command";
		target.setState(CurrentTarget::State::SETHEADING);
		return true;
	}
 	if (myRoad.isBlocked() == true)		//Road BLOCKED, go to BLOCKED state and wait it the obstacle moves
 	{
		controller->stopTheRobot(omnirobot_proxy);
 		//currentTargetBack.setTranslation(innerModel->transform("world", QVec::vec3(0, 0, -250), "robot"));
 		target.setState(CurrentTarget::State::BLOCKED);
		return false;
 	}

	// Get here when robot is stuck
// 	if(myRoad.requiresReplanning == true)
// 	{
// 	 		//qDebug() << __FUNCTION__ << "STUCK, PLANNING REQUIRED";
// 	 		//computePlan(innerModel);
// 	}
	
	//////////////////////////////////////////
	// Check if there is a plan for the target
	//////////////////////////////////////////
	bool coolPlan = true;
	if (target.isWithoutPlan() == true)
	{
		state.setState("PLANNING");
		QVec localT = target.getTranslation();
		coolPlan = plannerPRM.computePath(localT, innerModel);
		if (coolPlan == false)
		{
			qDebug() << __FUNCTION__ << "Path NOT found. Resetting to IDLE state";
			target.setState(CurrentTarget::State::STOP);
			return false;
		}
		target.setTranslation(localT);
		//qDebug() << __FUNCTION__ << "Plan obtained of " << planner->getPath().size() << " points";
		// take inner to current values
		updateInnerModel(innerModel, state);
		target.setWithoutPlan(false);

		//Init road   REMOVE TRASH FROM HERE
		myRoad.reset();
		myRoad.readRoadFromList(plannerPRM.getPath());
		myRoad.requiresReplanning = false;
		myRoad.computeDistancesToNext();
		myRoad.startRoad();
		state.setPlanningTime(reloj.elapsed());
		state.setState("EXECUTING");
	}

	///////////////////////////////////
	// Update the band
	/////////////////////////////////
	elasticband.update(innerModel, myRoad, laserData, target);

	///////////////////////////////////
	// compute all measures relating the robot to the road
	/////////////////////////////////
	myRoad.update();

	//myRoad.printRobotState(innerModel, target);

	/////////////////////////////////////////////////////
	//move the robot according to the current force field
	//////////////////////////////////////////////////////
	controller->update(innerModel, lData, omnirobot_proxy, myRoad);

	
	#ifdef USE_QTGUI
		waypointsdraw.draw(myRoad, viewer,  target);
	#endif
	
	state.setEstimatedTime(myRoad.getETA());
	return true;
}

bool LineFollower::update(InnerModel *innerModel, RoboCompLaser::TLaserData &laserData, RoboCompOmniRobot::OmniRobotPrx omnirobot_proxy, WayPoints &road)
{
	static QTime reloj = QTime::currentTime();   //TO be used for a more accurate control (predictive).
	/*static*/ long epoch = 100;
	static float lastVadvance = 0.f;
	const float umbral = 25.f;	//salto maximo de velocidad
	static float lastVrot = 0.f;
	const float umbralrot = 0.08f;	//salto maximo de rotación

	//Estimate the space that will be blindly covered and reduce Adv speed to remain within some boundaries
	//qDebug() << __FILE__ << __FUNCTION__ << "entering update with" << road.at(road.getIndexOfClosestPointToRobot()).pos;

	//Check robot state
	if( (road.isFinished() == true ) or (road.requiresReplanning== true) or (road.isLost == true))
	{
		 		if( road.isFinished() ) qDebug() << "road finished";
		 		if( road.requiresReplanning ) qDebug() << "requiresReplanning";
		 		if( road.isLost ) qDebug() << "robot is lost";
		stopTheRobot(omnirobot_proxy);

		return false;
	}

	///CHECK ROBOT INMINENT COLLISION
	float vside = 0;
	int j=0;
	road.setBlocked(false);
	for(auto i : laserData)
	{
		//printf("laser dist %f || baseOffsets %f \n",i.dist,baseOffsets[j]);
		if(i.dist < 10) i.dist = 30000;
		if( i.dist < baseOffsets[j] + 50 )
		{
			if(i.angle>-1.30 && i.angle<1.30){
			qDebug() << __FILE__ << __FUNCTION__<< "Robot stopped to avoid collision because distance to obstacle is less than " << baseOffsets[j] << " "<<i.dist << " " << i.angle;
			stopTheRobot(omnirobot_proxy);
			road.setBlocked(true);		//AQUI SE BLOQUEA PARA REPLANIFICAR
			qDebug()<<"DETECTADO OBSTACULO, REPLANIFICANDO";
 			break;
			}
		}
		else
		{
			if (i.dist < baseOffsets[j] + 150) 
			{
				if (i.angle > 0)
				{
					vside  = -80;
				}
				else
				{
					vside = 80;
				}
			}
		}
		j++;
	}

	/////////////////////////////////////////////////
	//////  CHECK CPU AVAILABILITY
	/////////////////////////////////////////////////
	if ( time.elapsed() > delay )   //Initial wait in secs so the robot waits for everything is setup. Maybe it could be moved upwards
	{
		float MAX_ADV_SPEED = 200.f;
		float MAX_ROT_SPEED = 0.3;
		if( (epoch-100) > 0 )				//Damp max speeds if elapsed time is too long
		{
			MAX_ADV_SPEED = 200 * exponentialFunction(epoch-100, 200, 0.2);
			MAX_ROT_SPEED = 0.3 * exponentialFunction(epoch-100, 200, 0.2);
		}
		float vadvance = 0;
		float vrot = 0;
		/////////////////////////////////////////////////
		//////   ROTATION SPEED
		////////////////////////////////////////////////

		// VRot is computed as the sum of three terms: angle with tangent to road + atan(perp. distance to road) + road curvature
		// as descirbed in Thrun's paper on DARPA challenge
		vrot = road.getAngleWithTangentAtClosestPoint() + atan( road.getRobotPerpendicularDistanceToRoad()/800.) + 0.8 * road.getRoadCurvatureAtClosestPoint() ;  //350->800.
	// Limiting filter
 		if( vrot > MAX_ROT_SPEED )
 			vrot = MAX_ROT_SPEED;
 		if( vrot < -MAX_ROT_SPEED )
 			vrot = -MAX_ROT_SPEED;

		/////////////////////////////////////////////////
		//////   ADVANCE SPEED
		////////////////////////////////////////////////

		// Factor to be used in speed control when approaching the end of the road
		float teta;
		if( road.getRobotDistanceToTarget() < 1000)
			teta = exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1);
		else
			teta= 1;
		
		// Factor to be used in speed control when approaching the end of the road
		

		//VAdv is computed as a reduction of MAX_ADV_SPEED by three computed functions:
//.........这里部分代码省略.........