/** * @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; }