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; }