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