本文整理汇总了C++中WayPoints::getRobotDistanceToTarget方法的典型用法代码示例。如果您正苦于以下问题：C++ WayPoints::getRobotDistanceToTarget方法的具体用法？C++ WayPoints::getRobotDistanceToTarget怎么用？C++ WayPoints::getRobotDistanceToTarget使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类WayPoints的用法示例。
在下文中一共展示了WayPoints::getRobotDistanceToTarget方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: update
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:
//.........这里部分代码省略.........