virtual void threadRelease() { // we require an immediate stop // before closing the client for safety reason arm->stopControl(); right_arm->stopControl(); // it's a good rule to restore the controller // context as it was before opening the module arm->restoreContext(startup_context_id); right_arm->restoreContext(startup_context_id); client.close(); clientR.close(); }
bool close() { igaze->restoreContext(startup_context_gaze); drvGaze.close(); iarm->restoreContext(startup_context_arm); drvArm.close(); imgLPortIn.close(); imgRPortIn.close(); imgLPortOut.close(); imgRPortOut.close(); rpcPort.close(); return true; }
bool close() { handlerPort.close(); objectsPort.close(); igaze->stopControl(); igaze->restoreContext(startupGazeContextID); igaze->deleteContext(startupGazeContextID); igaze->deleteContext(currentGazeContextID); if (clientGazeCtrl.isValid()) clientGazeCtrl.close(); icartLeft->stopControl(); icartLeft->restoreContext(startupArmLeftContextID); icartLeft->deleteContext(startupArmLeftContextID); icartLeft->deleteContext(currentArmLeftContextID); if (clientArmLeft.isValid()) clientArmLeft.close(); icartRight->stopControl(); icartRight->restoreContext(startupArmRightContextID); icartRight->deleteContext(startupArmRightContextID); icartRight->deleteContext(currentArmRightContextID); if (clientArmLeft.isValid()) clientArmLeft.close(); itorsoVelocity->stop(); if (clientTorso.isValid()) clientTorso.close(); return true; }
virtual void threadRelease() { armRest(); gazeRest(); igaze->restoreContext(startup_gazeContext_id); clientGaze.close(); if (arm!="none") { iarm->restoreContext(startup_armContext_id); clientArm.close(); } portImgIn.interrupt(); portImgOut.interrupt(); portTopDown.interrupt(); portSetFace.interrupt(); portImgIn.close(); portImgOut.close(); portTopDown.close(); portSetFace.close(); }
bool close() { iarm->stopControl(); iarm->restoreContext(startup_context); drvCart.close(); ivel->stop(joints.size(),joints.getFirst()); for (size_t i=0; i<modes.size(); i++) modes[i]=VOCAB_CM_POSITION; imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst()); drvHand.close(); if (simulator) { Bottle cmd,reply; cmd.addString("world"); cmd.addString("del"); cmd.addString("all"); simPort.write(cmd,reply); simPort.close(); } if (gaze) { igaze->stopControl(); drvGaze.close(); } igeo->stopFeedback(); igeo->setTransformation(eye(4,4)); drvGeomagic.close(); forceFbPort.close(); return true; }
void CalibModule::postureHelper(const Vector &gaze_ang, const Matrix &targetL, const Matrix &targetR) { IControlMode2 *imod; IPositionControl *ipos; ICartesianControl *icart; int ctxtL,ctxtR; Vector dof; yInfo("looking at target"); igaze->lookAtAbsAngles(gaze_ang); if (useArmL) { drvArmL.view(imod); drvArmL.view(ipos); openHand(imod,ipos); drvCartL.view(icart); icart->storeContext(&ctxtL); icart->getDOF(dof); dof=1.0; icart->setDOF(dof,dof); icart->setLimits(0,0.0,0.0); icart->setLimits(1,0.0,0.0); icart->setLimits(2,0.0,0.0); icart->setTrajTime(1.0); yInfo("reaching for left target"); icart->goToPoseSync(targetL.getCol(3),dcm2axis(targetL)); } if (useArmR) { drvArmR.view(imod); drvArmR.view(ipos); openHand(imod,ipos); drvCartR.view(icart); icart->storeContext(&ctxtR); icart->getDOF(dof); dof=1.0; dof[0]=dof[1]=dof[2]=0.0; icart->setDOF(dof,dof); icart->setTrackingMode(true); icart->setTrajTime(1.0); yInfo("reaching for right target"); icart->goToPoseSync(targetR.getCol(3),dcm2axis(targetR)); yInfo("waiting for right arm... "); icart->waitMotionDone(); yInfo("done"); } if (useArmL) { drvCartL.view(icart); yInfo("waiting for left arm... "); icart->waitMotionDone(); yInfo("done"); icart->restoreContext(ctxtL); icart->deleteContext(ctxtL); } if (useArmR) { drvCartR.view(icart); icart->restoreContext(ctxtR); icart->deleteContext(ctxtR); } }
bool configure(ResourceFinder &rf) { string name=rf.check("name",Value("teleop-icub")).asString().c_str(); string robot=rf.check("robot",Value("icub")).asString().c_str(); string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str(); double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble(); part=rf.check("part",Value("right_arm")).asString().c_str(); simulator=rf.check("simulator",Value("off")).asString()=="on"; gaze=rf.check("gaze",Value("off")).asString()=="on"; minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble()); maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble()); bool torso=rf.check("torso",Value("on")).asString()=="on"; Property optGeo("(device hapticdeviceclient)"); optGeo.put("remote",("/"+geomagic).c_str()); optGeo.put("local",("/"+name+"/geomagic").c_str()); if (!drvGeomagic.open(optGeo)) return false; drvGeomagic.view(igeo); if (simulator) { simPort.open(("/"+name+"/simulator:rpc").c_str()); if (!Network::connect(simPort.getName().c_str(),"/icubSim/world")) { yError("iCub simulator is not running!"); drvGeomagic.close(); simPort.close(); return false; } } if (gaze) { Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local",("/"+name+"/gaze").c_str()); if (!drvGaze.open(optGaze)) { drvGeomagic.close(); simPort.close(); return false; } drvGaze.view(igaze); } Property optCart("(device cartesiancontrollerclient)"); optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str()); optCart.put("local",("/"+name+"/cartesianController/"+part).c_str()); if (!drvCart.open(optCart)) { drvGeomagic.close(); if (simulator) simPort.close(); if (gaze) drvGaze.close(); return false; } drvCart.view(iarm); Property optHand("(device remote_controlboard)"); optHand.put("remote",("/"+robot+"/"+part).c_str()); optHand.put("local",("/"+name+"/"+part).c_str()); if (!drvHand.open(optHand)) { drvGeomagic.close(); if (simulator) simPort.close(); if (gaze) drvGaze.close(); drvCart.close(); return false; } drvHand.view(imod); drvHand.view(ipos); drvHand.view(ivel); iarm->storeContext(&startup_context); iarm->restoreContext(0); Vector dof(10,1.0); if (!torso) dof[0]=dof[1]=dof[2]=0.0; else dof[1]=0.0; iarm->setDOF(dof,dof); iarm->setTrajTime(Tp2p); Vector accs,poss; for (int i=0; i<9; i++) { joints.push_back(7+i); modes.push_back(VOCAB_CM_POSITION); accs.push_back(1e9); vels.push_back(100.0); poss.push_back(0.0); } poss[0]=20.0; poss[1]=70.0; imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst()); ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data()); ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data()); ipos->positionMove(joints.size(),joints.getFirst(),poss.data()); joints.clear(); modes.clear(); vels.clear(); for (int i=2; i<9; i++) { joints.push_back(7+i); modes.push_back(VOCAB_CM_VELOCITY); vels.push_back(40.0); } vels[vels.length()-1]=100.0; s0=s1=idle; c0=c1=0; onlyXYZ=true; stateStr[idle]="idle"; stateStr[triggered]="triggered"; stateStr[running]="running"; Matrix T=zeros(4,4); T(0,1)=1.0; T(1,2)=1.0; T(2,0)=1.0; T(3,3)=1.0; igeo->setTransformation(SE3inv(T)); igeo->setCartesianForceMode(); igeo->getMaxFeedback(maxFeedback); Tsim=zeros(4,4); Tsim(0,1)=-1.0; Tsim(1,2)=1.0; Tsim(1,3)=0.5976; Tsim(2,0)=-1.0; Tsim(2,3)=-0.026; Tsim(3,3)=1.0; pos0.resize(3,0.0); rpy0.resize(3,0.0); x0.resize(3,0.0); o0.resize(4,0.0); if (simulator) { Bottle cmd,reply; cmd.addString("world"); cmd.addString("mk"); cmd.addString("ssph"); // radius cmd.addDouble(0.02); // position cmd.addDouble(0.0); cmd.addDouble(0.0); cmd.addDouble(0.0); // color cmd.addInt(1); cmd.addInt(0); cmd.addInt(0); // collision cmd.addString("FALSE"); simPort.write(cmd,reply); } forceFbPort.open(("/"+name+"/force-feedback:i").c_str()); feedback.resize(3,0.0); return true; }
bool configure(yarp::os::ResourceFinder &rf) { Vector newDof, curDof; cout<<"Configuring module!"<<endl; moduleName=rf.check("name",Value("torsoModule")).asString().c_str(); robotName=rf.check("robot",Value("icub")).asString().c_str(); OPCName=rf.check("OPC",Value("memory")).asString().c_str(); period=rf.check("period",Value(0.2)).asDouble(); kp=rf.check("kp",Value(KP)).asDouble(); maxTorsoTrajTime=rf.check("torsoTime",Value(MAX_TORSO_TRAJ_TIME)).asDouble(); maxTorsoVelocity=rf.check("maxTorsoVelocity",Value(MAX_TORSO_VELOCITY)).asDouble(); maxArmTrajTime=rf.check("armTime",Value(MAX_ARM_TRAJ_TIME)).asDouble(); handlerPort.open(("/"+moduleName+"/rpc:i").c_str()); attach(handlerPort); objectsPort.open(("/"+moduleName+"/OPC:io").c_str()); if(!objectsPort.addOutput(("/"+OPCName+"/rpc").c_str())){ cout<<"Error connecting to OPC client!"<<endl; return false; } Property optionGaze; optionGaze.put("device","gazecontrollerclient"); optionGaze.put("remote","/iKinGazeCtrl"); optionGaze.put("local",("/"+moduleName+"/gaze").c_str()); if(!clientGazeCtrl.open(optionGaze)){ cout<<"Error opening gaze client!"<<endl; return false; } clientGazeCtrl.view(igaze); igaze->restoreContext(0); igaze->storeContext(&startupGazeContextID); gazeHomePosition.push_back(GAZE_HOME_POS_X); gazeHomePosition.push_back(GAZE_HOME_POS_Y); gazeHomePosition.push_back(GAZE_HOME_POS_Z); Property leftArmOption; leftArmOption.put("device","cartesiancontrollerclient"); leftArmOption.put("remote",("/"+robotName+"/cartesianController/left_arm").c_str()); leftArmOption.put("local",("/"+moduleName+"/left_arm").c_str()); if(!clientArmLeft.open(leftArmOption)){ cout<<"Error opening left arm client!"<<endl; return false; } clientArmLeft.view(icartLeft); icartLeft->restoreContext(0); icartLeft->storeContext(&startupArmLeftContextID); leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_X); leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_Y); leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_Z); icartLeft->setTrajTime(maxArmTrajTime); icartLeft->storeContext(¤tArmLeftContextID); Property rightArmOption; rightArmOption.put("device","cartesiancontrollerclient"); rightArmOption.put("remote",("/"+robotName+"/cartesianController/right_arm").c_str()); rightArmOption.put("local",("/"+moduleName+"/right_arm").c_str()); if(!clientArmRight.open(rightArmOption)){ cout<<"Error opening right arm client!"<<endl; return false; } clientArmRight.view(icartRight); icartRight->restoreContext(0); icartRight->storeContext(&startupArmRightContextID); rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_X); rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_Y); rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_Z); icartRight->setTrajTime(maxArmTrajTime); icartRight->storeContext(¤tArmRightContextID); computeArmOr(); Property torsoOptions; torsoOptions.put("device", "remote_controlboard"); torsoOptions.put("remote",("/"+robotName+"/torso").c_str()); torsoOptions.put("local",("/"+moduleName+"/torso").c_str()); if(!clientTorso.open(torsoOptions)){ cout<<"Error opening torso client!"<<endl; return false; } clientTorso.view(itorsoVelocity); clientTorso.view(itorsoMode); torsoHomePosition.push_back(TORSO_HOME_POS_ROLL); torsoHomePosition.push_back(TORSO_HOME_POS_PITCH); torsoHomePosition.push_back(TORSO_HOME_POS_YAW); torsoAcceleration.push_back(TORSO_ACCELERATION_ROLL); torsoAcceleration.push_back(TORSO_ACCELERATION_PITCH); torsoAcceleration.push_back(TORSO_ACCELERATION_YAW); clientTorso.view(iTorsoEncoder); waypoints.resize(3,3); waypoints(0,0) = 0.0; waypoints(0,1) = -15.0; waypoints(0,2) = 10.0; waypoints(1,0) = 0.0; waypoints(1,1) = 15.0; waypoints(1,2) = 10.0; waypoints(2,0) = 0.0; waypoints(2,1) = 0.0; waypoints(2,2) = 20.0; index = 0; running = false; /*Bottle bAdd, bReply; bAdd.addVocab(Vocab::encode("add")); Bottle &bTempAdd=bAdd.addList(); Bottle &bEntity=bTempAdd.addList(); bEntity.addString("entity"); bEntity.addString("action"); Bottle &bName=bTempAdd.addList(); bName.addString("name"); bName.addString("ball"); Bottle &bX= bTempAdd.addList(); bX.addString("position_3d"); Bottle &coord = bX.addList(); coord.addDouble(-0.05); coord.addDouble(0.0); coord.addDouble(0.2); objectsPort.write(bAdd,bReply); cout<<bReply.get(0).asVocab()<<endl; */ return true; }
bool respond(const Bottle& command, Bottle& reply) { if(command.size()==0) { reply.addString("No command received."); return true; } switch(command.get(0).asVocab()){ case HOME: if(command.size()>1) switch(command.get(1).asVocab()){ case ARM: int tempCxL,tempCxR; icartLeft->storeContext(&tempCxL); icartLeft->storeContext(&tempCxR); icartLeft->restoreContext(currentArmLeftContextID); icartLeft->restoreContext(currentArmRightContextID); icartLeft->goToPositionSync(leftArmHomePosition); icartRight->goToPositionSync(rightArmHomePosition); icartRight->waitMotionDone(0.1,2); icartLeft->waitMotionDone(0.1,2); icartLeft->restoreContext(tempCxR); icartLeft->restoreContext(tempCxL); icartLeft->deleteContext(tempCxR); icartLeft->deleteContext(tempCxL); reply.addString("Arm home position reached."); return true; case TORSO: exploreTorso(torsoHomePosition); reply.addString("Torso home position reached."); return true; case GAZE: int tempCx; igaze->storeContext(&tempCx); igaze->restoreContext(currentGazeContextID); igaze->lookAtFixationPoint(gazeHomePosition); igaze->waitMotionDone(0.1,2); igaze->restoreContext(tempCx); igaze->deleteContext(tempCx); reply.addString("Gaze home position reached."); return true; default: reply.addString("Wrong device for home position."); return true; } else{ int tempCxL,tempCxR,tempCx; icartLeft->storeContext(&tempCxL); icartLeft->storeContext(&tempCxR); icartLeft->restoreContext(currentArmLeftContextID); icartLeft->restoreContext(currentArmRightContextID); igaze->storeContext(&tempCx); igaze->restoreContext(currentGazeContextID); icartLeft->goToPositionSync(leftArmHomePosition); icartRight->goToPositionSync(rightArmHomePosition); igaze->lookAtFixationPoint(gazeHomePosition); exploreTorso(torsoHomePosition); igaze->waitMotionDone(0.1,2); icartRight->waitMotionDone(0.1,2); icartLeft->waitMotionDone(0.1,2); icartLeft->restoreContext(tempCxR); igaze->restoreContext(tempCx); icartLeft->restoreContext(tempCxL); icartLeft->deleteContext(tempCxR); icartLeft->deleteContext(tempCxL); igaze->deleteContext(tempCx); reply.addString("Ok, moving to home position."); return true; } case LOOK_AT: if(command.size()==2){ Bottle bAsk,bReply, bGet; bAsk.addVocab(Vocab::encode("ask")); Bottle &bTempAsk=bAsk.addList().addList(); bTempAsk.addString("name"); bTempAsk.addString("=="); bTempAsk.addString(command.get(1).asString()); objectsPort.write(bAsk,bReply); cout<<"first"<<endl; if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false || bReply.get(1).asList()->find("id").asList()->size()==0){ reply.addVocab(Vocab::encode("nack")); return true; } bGet.addVocab(Vocab::encode("get")); Bottle &bTempGet=bGet.addList().addList(); bTempGet.addString("id"); bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt()); objectsPort.write(bGet,bReply); cout<<"second"<<endl; if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_3d")==false || bReply.get(1).asList()->find("position_3d").asList()->size()==0){ reply.addVocab(Vocab::encode("nack")); return true; } cout<<"third"<<endl; Vector gazePosition(3); gazePosition[0] = bReply.get(1).asList()->find("position_3d").asList()->get(0).asDouble(); gazePosition[1] = bReply.get(1).asList()->find("position_3d").asList()->get(1).asDouble(); gazePosition[2] = bReply.get(1).asList()->find("position_3d").asList()->get(2).asDouble(); int tempCx; igaze->storeContext(&tempCx); igaze->restoreContext(currentGazeContextID); igaze->lookAtFixationPoint(gazePosition); igaze->waitMotionDone(0.2,3); igaze->restoreContext(tempCx); igaze->deleteContext(tempCx); reply.addString("Gaze position reached."); return true; } else if(command.size()==4){ Vector gazePosition(3); int tempCx; gazePosition[0] = command.get(1).asDouble(); gazePosition[1] = command.get(2).asDouble(); gazePosition[2] = command.get(3).asDouble(); igaze->storeContext(&tempCx); igaze->restoreContext(currentGazeContextID); igaze->lookAtFixationPoint(gazePosition); igaze->waitMotionDone(0.2,3); igaze->restoreContext(tempCx); igaze->deleteContext(tempCx); reply.addString("Gaze position reached."); return true; } else{ reply.addString("Wrong number of parameters for lookAt."); return true; } case GET: if(command.size()==2){ Bottle bAsk,bReply, bGet; bAsk.addVocab(Vocab::encode("ask")); Bottle &bTempAsk=bAsk.addList().addList(); bTempAsk.addString("name"); bTempAsk.addString("=="); bTempAsk.addString(command.get(1).asString()); objectsPort.write(bAsk,bReply); if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false || bReply.get(1).asList()->find("id").asList()->size()==0){ reply.addVocab(Vocab::encode("nack")); return true; } bGet.addVocab(Vocab::encode("get")); Bottle &bTempGet=bGet.addList().addList(); bTempGet.addString("id"); bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt()); objectsPort.write(bGet,bReply); if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_2d_left")==false || bReply.get(1).asList()->find("position_2d_left").asList()->size()==0){ reply.addVocab(Vocab::encode("nack")); return true; } Vector objPosition(4); objPosition[0] = bReply.get(1).asList()->find("position_2d_left").asList()->get(0).asInt(); objPosition[1] = bReply.get(1).asList()->find("position_2d_left").asList()->get(1).asInt(); objPosition[2] = bReply.get(1).asList()->find("position_2d_left").asList()->get(2).asInt(); objPosition[3] = bReply.get(1).asList()->find("position_2d_left").asList()->get(3).asInt(); reply.addVocab(VOCAB3('a','c','k')); Bottle &coord = reply.addList(); coord.addInt((int)(objPosition[0]+objPosition[2])/2); coord.addInt((int)(objPosition[1]+objPosition[3])/2); return true; } else{ reply.addString("Wrong number of parameters for get."); return true; } case TRACK: if(command.size()==3) switch(command.get(1).asVocab()){ case ARM: if (command.get(2).asString() == "on"){ int tempCxL,tempCxR; //icartLeft->storeContext(&tempCxL); //icartLeft->storeContext(&tempCxR); //icartLeft->restoreContext(currentArmLeftContextID); //icartLeft->restoreContext(currentArmRightContextID); icartLeft->setTrackingMode(true); icartRight->setTrackingMode(true); icartRight->storeContext(¤tArmRightContextID); icartLeft->storeContext(¤tArmLeftContextID); //icartLeft->restoreContext(tempCxR); // icartLeft->restoreContext(tempCxL); // icartLeft->deleteContext(tempCxR); // icartLeft->deleteContext(tempCxL); reply.addString("Arm tracking mode enabled."); } else if (command.get(2).asString() == "off"){ int tempCxL,tempCxR; //icartLeft->storeContext(&tempCxL); //icartLeft->storeContext(&tempCxR); //icartLeft->restoreContext(currentArmLeftContextID); //icartLeft->restoreContext(currentArmRightContextID); icartLeft->setTrackingMode(false); icartRight->setTrackingMode(false); icartRight->storeContext(¤tArmRightContextID); icartLeft->storeContext(¤tArmLeftContextID); //icartLeft->restoreContext(tempCxR); // icartLeft->restoreContext(tempCxL); // icartLeft->deleteContext(tempCxR); // icartLeft->deleteContext(tempCxL); reply.addString("Arm tracking mode disabled."); } else reply.addString("Wrong parameter: on/off"); return true; case GAZE: if (command.get(2).asString() == "on"){ int tempCx; //igaze->storeContext(&tempCx); //igaze->restoreContext(currentGazeContextID); igaze->setTrackingMode(true); igaze->storeContext(¤tGazeContextID); //igaze->restoreContext(tempCx); //igaze->deleteContext(tempCx); reply.addString("Gaze tracking mode enabled."); } else if (command.get(2).asString() == "off"){ int tempCx; //igaze->storeContext(&tempCx); //igaze->restoreContext(currentGazeContextID); igaze->setTrackingMode(false); igaze->storeContext(¤tGazeContextID); //igaze->restoreContext(tempCx); //igaze->deleteContext(tempCx); reply.addString("Gaze tracking mode disabled."); } else reply.addString("Wrong parameter for trac: on/off"); return true; default: reply.addString("Wrong device for tracking mode."); return true; } else{ reply.addString("Missing parameters for track."); return true; } case BLOCK: if(command.size()>1) switch(command.get(1).asVocab()){ case GAZE: if(command.size()==3){ // int tempCx; //igaze->storeContext(&tempCx); //igaze->restoreContext(currentGazeContextID); igaze->blockEyes(command.get(2).asDouble()); igaze->storeContext(¤tGazeContextID); //igaze->restoreContext(tempCx); //igaze->deleteContext(tempCx); reply.addString("Gaze blocking mode enabled."); } else{ //int tempCx; //igaze->storeContext(&tempCx); //igaze->restoreContext(currentGazeContextID); igaze->blockEyes(DEFAULT_VERGENCE); igaze->storeContext(¤tGazeContextID); //igaze->restoreContext(tempCx); //igaze->deleteContext(tempCx); reply.addString("Default vergence set."); } return true; default: reply.addString("Wrong device for blocking mode."); return true; } else{ reply.addString("Missing parameters for block."); return true; } case GOTO: if(command.size()==4){ Vector torsoTarget(3); torsoTarget[0] = command.get(1).asDouble(); torsoTarget[1] = command.get(2).asDouble(); torsoTarget[2] = command.get(3).asDouble(); exploreTorso(torsoTarget); reply.addString("Torso position reached."); } else{ reply.addString("Missing parameters for goto."); return true; } return true; case RUN: running = true; reply.addString("Ready for exploration. next or stop?"); return true; case NEXT: if(running){ exploreTorso(waypoints.getRow(index)); index++; if (index > 2){ running = false; index = 0; reply.addString("Waypoint reached. End of waypoints."); } else{ reply.addString("Waypoint reached. next or stop?"); } } else reply.addString("Not running."); return true; case STOP: index = 0; running = false; reply.addString("Run stopped. Index reset."); return true; default: RFModule::respond(command,reply); return true; } return true; }