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; }
void Monofin::on_actionCleanPolygon_triggered() { cleanPoints(); }