private int AskForName(int currentState, object o)
{
foundHuman = brain.WaitForHumanOrders("Please, tell me your name in the form: robot, my name is:", 15000, true, knownPersons);
if ((foundHuman == null || foundHuman == "") && attemptCounter < 2)
{
attemptCounter++;
return(currentState);
}
attemptCounter = 0;
if (rejectedNames.Contains(foundHuman))
{
brain.SayAsync("Sorry the name " + foundHuman + " is already in my system");
return(currentState);
}
if (foundHuman == "" || foundHuman == null)
{
foundHuman = defaultName;
brain.SayAsync("Sorry, I cannot understand your name. You will be " + foundHuman + " for the remainder of the test");
Thread.Sleep(1500);
}
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Confirm \"yes\" received");
brain.SayAsync("O.K. I am going to remember your face. Please look straight to my web camera.");
Thread.Sleep(500);
//if(trainShirt)
// return (int)States.TrainShirt;
return((int)States.TrainFace);
}
private int EsperandoPersonaEnFrente(int currentState, object o)
{
List <Vector3> pier;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\EsperandoPersonaEnFrente.-> Esperando a que la persona se digne a ponerse en frente");
do
{
Thread.Sleep(500);
this.cmdMan.MVN_PLN_findlegs("15", out pier, 2000);
} while ((pier.Count == 0));
bool sal = false;
for (int i = 0; i < pier.Count; i++)
{
if ((Math.Pow(pier[i].X, 2) + Math.Pow(pier[i].Y, 2)) < 2.25)
{
hum = pier[i];
sal = true;
}
}
if (sal)
{
Thread.Sleep(500);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\EsperandoPersonaEnFrente.-> Por fin Aparece");
brain.SayAsync("Hello human, I am the robot justina.");
return((int)States.SensandoPersona);
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\EsperandoPersonaEnFrente.-> Ups no era");
return((int)States.EsperandoPersonaEnFrente);
}
}
private int InitialState(int currentState, object o)
{
brain.lastRecoGestures.Clear();
cmdMan.HEAD_lookat(0.0, -0.2, 5000);
brain.SayAsync("I am looking for requests, please raise your hand above your head.");
return((int)(States.SearchRequest));
}
private int SubiendoMisBrazos(int currentState, object o)
{
this.cmdMan.ARMS_goto("standby", 8000);
//this.cmdMan.HEAD_lookat();
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Subiendo mis brazos");
brain.SayAsync("Wait for my signal to walk.");
return((int)States.EsperandoPersonaEnFrente);
}
private int AskForName(int currentState, object o)
{
//d this.cmdMan.SPG_GEN_say("Please tell your name", 10000);
foundHuman = brain.WaitForHumanOrders("Please tell me your name", 60000, true, knownPersons);
if (rejectedNames.Contains(foundHuman))
{
brain.SayAsync("Sorry the name + " + foundHuman + " is already in my system");
return(currentState);
}
if (foundHuman == null)
{
foundHuman = defaultName;
this.cmdMan.SPG_GEN_say("I cannot understand your name. You will be " + foundHuman + "creative, for the remainder of the test");
}
//rejectedNames.Add(foundHuman);
return((int)States.AsociateName);
}
int TakeObject(int currentState, object o)
{
if (ObjectToFind.Length >= 7 && ObjectToFind.Substring(0, 7).ToLower().Equals("unknown"))
{
SayObjectName = "unknown object";
}
else
{
SayObjectName = ObjectToFind;
}
brain.SayAsync("I will take the " + SayObjectName);
Thread.Sleep(5000);
if (cmdMan.ST_PLN_takeobject(ObjectToFind, 180000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("object taken");
}
else
{
if (useTakeHandOver)
{
TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover");
brain.SayAsync(" I cant reach the " + SayObjectName);
cmdMan.ST_PLN_takehandover(ObjectToFind, 40000);
}
else
{
brain.SayAsync("I cant reach the " + SayObjectName + ", I will look for another object.");
objectsFound.RemoveAt(objectFoundIndex);
TextBoxStreamWriter.DefaultLog.WriteLine("Cant take the object, trying to take another one.");
return((int)States.SetObjectToTake);
}
}
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
int TakeObject(int currentState, object o)
{
if (ObjectToFind.Substring(0, 7).ToLower().Equals("unknown"))
{
SayObjectName = "unknown object";
}
else
{
SayObjectName = ObjectToFind;
}
if (this.succesGetClose)
{
brain.SayAsync("I will take the " + SayObjectName);
Thread.Sleep(5000);
if (cmdMan.ST_PLN_takeobject(ObjectToFind, 105000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("object taken");
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover");
brain.SayAsync(" I cant take the " + SayObjectName);
cmdMan.ST_PLN_takehandover(ObjectToFind, 40000);
}
}
else
{
Thread.Sleep(5000);
TextBoxStreamWriter.DefaultLog.WriteLine("Getclose not succesfull, using takehandover");
brain.SayAsync("I can't take the " + SayObjectName);
cmdMan.ST_PLN_takehandover(ObjectToFind, 40000);
}
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
int TakeObject(int currentState, object o)
{
string leftArm = "", rightArm = "";
if (firstObjectTaken)
{
armToUse = (laTaken) ? "right" : "left";
}
else
{
armToUse = "";
}
armToUse = "left";
if (objectsToFind[takeCounter].Length >= 7 && objectsToFind[takeCounter].Substring(0, 7).ToLower().Equals("unknown"))
{
SayObjectName = "unknown object";
}
else
{
SayObjectName = objectsToFind[takeCounter];
}
//TEMP
//brain.SayAsync("I will take the object");
//Thread.Sleep(10000);
//reemplazar por bnuevo comando para forzar a que tome un objeto con un brazo dado
//if (cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], armToUse, out string, 180000))
//if (cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], 180000))
cmdMan.ST_PLN_takeobject(objectsToFind[takeCounter], armToUse, out leftArm, out rightArm, 180000);
if (!(leftArm == "empty" && rightArm == "empty"))
{
brain.SayAsync("I got the " + SayObjectName);
TextBoxStreamWriter.DefaultLog.WriteLine("object taken: " + objectsToFind[takeCounter]);
foundObjects[takeCounter] = objectsToFind[takeCounter];
//arctualizar la bandera del brazo cupado laTaken}
laTaken = (leftArm == "empty") ? false : true;
if (!firstObjectTaken)
{
armsOrder[0] = (laTaken) ? "left" : "right";
}
else
{
armsOrder[1] = (armsOrder[0] == "right") ? "left" : "right";
}
TextBoxStreamWriter.DefaultLog.WriteLine(objectsToFind[takeCounter] + " using the " + ((!firstObjectTaken) ? armsOrder[0] : armsOrder[1]) + " arm");
}
else
{
if (useTakeHandOver)
{
TextBoxStreamWriter.DefaultLog.WriteLine("cant take object, using takehandover");
brain.SayAsync(" I cant reach the " + SayObjectName);
//brain.SayAsync("I will point at the " + objectsToFind[takeCounter] + ".");
//TODO: point at object
cmdMan.ST_PLN_takehandover(objectsToFind[takeCounter], 40000);
foundObjects[takeCounter] = objectsToFind[takeCounter];
//arctualizar la bandera del brazo cupado laTaken
laTaken = (leftArm == "empty") ? false : true;
if (!firstObjectTaken)
{
armsOrder[0] = (laTaken) ? "left" : "right";
}
else
{
armsOrder[1] = (armsOrder[0] == "right") ? "left" : "right";
}
}
else
{
finalState = FinalStates.FoundButNotTaken;
foundObjects[takeCounter] = "";
//TEMP
//brain.SayAsync("I cant reach the " + SayObjectName + ", I will look for another object.");
//brain.SayAsync("I cant reach the object, I will look for another object.");
TextBoxStreamWriter.DefaultLog.WriteLine("Cant take the object, trying to take another one.");
objectsFound.RemoveAt(0);
return((int)States.SetObjectToTake);
}
}
if (!firstObjectTaken && twoArms)
{
TextBoxStreamWriter.DefaultLog.WriteLine("First object Taken.");
/*despues de la primer ejecucion checar con que brazo tomó el objeto y actualizar la bandera laTaken(true, toma con el brazo izq, else si no )*/
//laTaken=false
takeCounter++;
firstObjectTaken = true;
objectsFound.RemoveAt(0);
return((int)States.SetObjectToTake);
}
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
private int CallRobot(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("Waiting for a person to call the robot...");
//string[] expectedStrigs = null; ;
string order;
if (!this.cmdMan.SPK_identify(2, out this.nameCallPerson, out this.angleCallPerson, 10000))
{
//Thread.Sleep(2000);
return(currentState);
}
string recognizedPerson = "human";
if (this.nameCallPerson == "unknown")
{
if (voiceRecognitionAttempts < 1)
{
voiceRecognitionAttempts++;
TextBoxStreamWriter.DefaultLog.WriteLine("Could not recognize person's voice, trying again.");
return(currentState);
}
else
{
this.cmdMan.HEAD_lookat(this.angleCallPerson, 0.0, 5000);
double pan, tilt;
//if (this.cmdMan.ST_PLN_findhuman("human", "hdtilt", 60000, out recognizedPerson) && recognizedPerson != "unknown")
if (this.cmdMan.PRS_FND_findhuman(ref recognizedPerson, out pan, out tilt, 60000) && recognizedPerson != "unknown")
{
this.cmdMan.HEAD_lookatrel(pan, tilt, 5000);
brain.SayAsync("Hello " + recognizedPerson);
TextBoxStreamWriter.DefaultLog.WriteLine("Person recognized with eyes: " + recognizedPerson);
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("Person was not recognized, neither by voice or face.");
brain.SayAsync("Hello human, I do not know you. Would you like me to remember you?");
voiceRecognitionAttempts = 0;
if (brain.WaitForUserConfirmation())
{
return((int)States.TrainFace);
}
return(currentState);
}
}
}
else
{
if (this.angleCallPerson > 0.17)
{
this.brain.SayAsync("Hello " + nameCallPerson + " I heard you in my left side.");
}
else if (this.angleCallPerson < -0.17)
{
this.brain.SayAsync("Hello " + nameCallPerson + " I heard you in my rigth side.");
}
else
{
this.brain.SayAsync("Hello " + nameCallPerson + " I heard you front of me.");
}
Thread.Sleep(3000);
//this.cmdMan.MVN_PLN_move(0, this.angleCallPerson, 3000);
this.cmdMan.HEAD_lookat(this.angleCallPerson, 0);
}
voiceRecognitionAttempts = 0;
this.brain.SayAsync("Do you need something?");
order = this.brain.WaitForHumanOrders("", 7000, true, "table", "no");
if (order == null || order == String.Empty || order == "no")
{
// this.cmdMan.HEAD_lookat(0, 0);
// this.cmdMan.MVN_PLN_move(0, -this.angleCallPerson, 3000);
return(currentState);
}
else
{
this.cmdMan.HEAD_lookat(0, 0);
return((int)States.AdjustTable);
}
}
/// <summary>
/// The robot start to follow a human (guide phase).
/// </summary>
private int FollowHuman(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> FollowHuman state reached.");
//start saving the robot's path (hansel & gretel)
cmdMan.MVN_PLN_startsavingpath();
cmdMan.HEAD_lookat(SMConfiguration.HeadHomePan, SMConfiguration.HeadHomeTilt, 10000);
//Thread.Sleep(500);
//cmdMan.HEAD_lookat(SMConfiguration.HeadFollowPan, SMConfiguration.HeadFollowTilt, 10000);
//send the arms to a position above the laser
cmdMan.ARMS_goto(SMConfiguration.ArmsFollowPosition);
brain.SayAsync(SMConfiguration.StartFollowMessage);
//start to follow a human
cmdMan.MVN_PLN_startfollowhuman();
brain.recognizedSentences.Clear();
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Waiting for location command");
return((int)States.WaitForLocationCommand);
}
private int WaitForOrder(int currentState, object o)
{
if (attemptCounter % 30 == 0)
{
brain.SayAsync("What would you like to order " + currentOrder.personName + "?");
}
SentenceImperative sentence;
if (brain.FindUserComplexCommand(out sentence))
{
if (sentence.ActionClass == VerbType.Bring)
{
currentOrder.objectName = sentence.DirectObject;
TextBoxStreamWriter.DefaultLog.WriteLine("recognized request : Bring me the " + currentOrder.objectName);
brain.SayAsync("Would you like me to bring you the " + currentOrder.objectName + "?");
attemptCounter = 0;
return((int)States.WaitForConfirmation);
}
}
Thread.Sleep(500);
attemptCounter++;
if (attemptCounter == 89)
{
attemptCounter = 0;
brain.SayAsync("I did not understand your order.");
currentOrder.objectName = defaultDrink;
TextBoxStreamWriter.DefaultLog.WriteLine("No order was recognized, default object: " + currentOrder.objectName);
brain.SayAsync("I will bring you the " + currentOrder.objectName);
return((int)States.EnqueueOrder);
}
return(currentState);
}