/** * @brief Stops the robot * * @return bool */ bool SpecificWorker::stopCommand(CurrentTarget &target, WayPoints &myRoad, TrajectoryState &state) { controller->stopTheRobot(omnirobot_proxy); myRoad.setFinished(true); myRoad.reset(); myRoad.endRoad(); #ifdef USE_QTGUI //myRoad.clearDraw(viewer->innerViewer); //drawGreenBoxOnTarget(target.getTranslation()); #endif target.reset(); state.setElapsedTime(taskReloj.elapsed()); state.setState("IDLE"); qDebug() << __FUNCTION__ << "Robot at pose:" << innerModel->transform6D("world", "robot"); 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; }