/**
* @brief Check if some point ahead on the road is closer (L2) than along the road, to take a shortcut
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::shortCut(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Compute distances from robot to all points ahead. If any of them is laser-visible and significantly shorter than de distance along de road, try it!
WayPoints::iterator robot = road.getIterToClosestPointToRobot();
WayPoints::iterator best = road.begin();
for (WayPoints::iterator it = robot + 1; it != road.end(); ++it)
{
//qDebug() << __FUNCTION__ << it->isVisible << (it->pos - robot->pos).norm2() << road.computeDistanceBetweenPointsAlongRoad(robot, it);
if ( it->isVisible )
{
if (road.computeDistanceBetweenPointsAlongRoad(robot, it) - (it->pos - robot->pos).norm2() > 300) //Half robot SACARRRR
{
qDebug() << __FUNCTION__ << "Candidato";
//Check if the robot passes through the straight line
if (checkCollisionAlongRoad(innermodel, laserData, road, robot, it, ROBOT_RADIUS))
{
//Is so remove all intermadiate points between robot and new subtarget
qDebug() << __FUNCTION__ << "Confirmado";
best = it;
}
}
}
else
break;
}
if (best != road.begin() and (robot + 1) != road.end())
road.erase(robot + 1, best);
return false;
}
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;
}