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;
}
예제 #3
0
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:
		//				* road curvature reduces forward speed
		//				* VRot reduces forward speed
		//				* reduction is 1 if there are not obstacle.
		//				* teta that applies when getting close to the target (1/roadGetCurvature)
		//				* a Delta that takes 1 if approaching the target is true, 0 otherwise. It applies only if at less than 1000m to the target
		vadvance = MAX_ADV_SPEED * exp(-fabs(1.6 * road.getRoadCurvatureAtClosestPoint()))
								 * exponentialFunction(vrot, 0.8, 0.01)
								 * teta;
                                                                                                                                //* exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1)
								 //* sunk;


		if(fabs(vrot - lastVrot) > umbralrot)
		{
			//qDebug()<<"lastrot "<<lastVrot << "\n vrot "<< vrot;
			if(vrot > lastVrot)
				vrot = lastVrot + umbralrot;
			else vrot = lastVrot - umbralrot;
		}
		lastVrot=vrot;

		//Pre-limiting filter to avoid displacements in very closed turns
		if( fabs(vrot) == 0.3)
			vadvance = 0;
			vside = 0;
		
		//stopping speed jump
		if(fabs(vadvance - lastVadvance) > umbral)
		{
			//qDebug()<<"lastadvanced "<<lastVadvance << "\n vadvance "<< vadvance;
			if(vadvance > lastVadvance)
				vadvance = lastVadvance + umbral;
			else vadvance = lastVadvance - umbral;
		}
		lastVadvance=vadvance;

 		// Limiting filter
 		if( vadvance > MAX_ADV_SPEED )
 			vadvance = MAX_ADV_SPEED;
		


		//vside = vrot*MAX_ADV_SPEED;
		
		/////////////////////////////////////////////////
		//////  LOWEST-LEVEL COLLISION AVOIDANCE CONTROL
		////////////////////////////////////////////////

		//bool collision = avoidanceControl(innerModel, laserData, vadvance, vrot);
//  		if( collision )
//  			road.setBlocked(true);

		/////////////////////////////////////////////////
		///  SIDEWAYS LASTMINUTE AVOIDING WITH THE OMNI BASE
		/////////////////////////////////////////////////
		//TODO: PROBAR EN URSUS A VER COMO QUEDA..
		
// 		std::sort(laserData.begin(), laserData.end(), [](auto a, auto b){ return a.dist < b.dist;});
// 		if(laserData.front().dist > 300 && vside == 0)// and fabs(laserData.front().angle)>0.3)
// 		{
// 			if( laserData.front().angle > 0) vside  = -30;
// 			else vside = 30;
// 		}
		
		/////////////////////////////////////////////////
		//////   EXECUTION
		////////////////////////////////////////////////

// 		qDebug() << "------------------LineFollower Report ---------------;";
// 		qDebug() <<"	VAdv: " << vadvance << "|\nVRot: " << vrot << "\nVSide: " << vside;
// 		qDebug() << "---------------------------------------------------;";
                
   		try { omnirobot_proxy->setSpeedBase(vside, vadvance, vrot);}
   		catch (const Ice::Exception &e) { std::cout << e << "Omni robot not responding" << std::endl; }
	}
	else		//Too long delay. Stopping robot.
	{	
		qDebug() << __FILE__ << __FUNCTION__ << "Processing delay" << epoch << "ms. too high. Stopping the robot for safety";
		try { omnirobot_proxy->setSpeedBase( 0, 0, 0);	}
		catch (const Ice::Exception &e) { std::cout << e << "Omni robot not responding" << std::endl; }
	}

	epoch = reloj.restart();  //epoch time in ms
	return false;

}