bool ElasticBand::update(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData,
const CurrentTarget ¤tTarget, 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:
// * 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;
}