int GetCloseObjectLocation(int currentState, object o)
{
cmdMan.HEAD_lookat(0, 0, 5000);
TextBoxStreamWriter.DefaultLog.WriteLine("Getting close to: " + this.objectLocation);
if (cmdMan.MVN_PLN_getclose(this.objectLocation, 600000))
{
cmdMan.SPG_GEN_asay("i have arrived to the " + this.objectLocation, 10000);
TextBoxStreamWriter.DefaultLog.WriteLine("Location reached: " + this.objectLocation);
this.attemptCounter = 0;
return((int)States.SearchCloseObjects);
}
if (attemptCounter < 3)
{
this.attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant get close to the specified location, trying again.");
/// para interface
cmdMan.SPG_GEN_asay("I can't get close to the" + this.objectLocation, 10000);
return(currentState);
}
//this.brain.SayAsync("I could not get close to the specified location");
this.attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant get close to the specified location, will try to continue with the test.");
return((int)States.SearchCloseObjects);
}
int FinalState(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("head to 0,0");
cmdMan.HEAD_lookat(0, 0, 10000);
cmdMan.MVN_PLN_move(-0.4, 0.0, 10000);
return(currentState);
}
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));
}
int FinalState(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("head to 0,0");
cmdMan.HEAD_lookat(0, 0, 10000);
this.finalState = FinalStates.OK;
return(currentState);
}
private int TrainShirt(int currentState, object o)
{
brain.SayAsync("Im going to remember your clothes.");
cmdMan.HEAD_lookat(0.0, -0.3);
Thread.Sleep(1000);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Remembering shirt...");
brain.SayAsync("Human, please stand one meter in front of me.");
Thread.Sleep(1000);
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_front", 3000))
{
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_front", 3000))
{
cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_front", 3000);
}
}
brain.SayAsync("Now, please turn to your left side.");
Thread.Sleep(2000);
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_left", 3000))
{
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_left", 3000))
{
cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_left", 3000);
}
}
brain.SayAsync("Now, turn your back towards me.");
Thread.Sleep(2000);
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_back", 3000))
{
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_back", 3000))
{
cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_back", 3000);
}
}
brain.SayAsync("Now, please turn to your right side.");
Thread.Sleep(2000);
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_right", 3000))
{
if (!cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_right", 3000))
{
cmdMan.OBJ_FNDT_trainshirt(foundHuman + "_right", 3000);
}
}
brain.SayAsync("Thank you.");
Thread.Sleep(500);
return((int)States.TrainFace);
//return (int)States.FinalState;
}
/// <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 SearchFall(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> SearchFall state of SM_DetectPersonFall reached.");
//double headAngle=-Math.PI/6;
double headAngle = (-0.5 * 3.1416) / 180;
cmdMan.HEAD_lookat(0.0, headAngle, 5000);
//headAngle = (headAngle * 180) / Math.PI;
headAngle = -0.5;
brain.lastPersonFallDetected.Clear();
Thread.Sleep(5000);
//enviar comando para que comienze a detectar caidas
cmdMan.VISION_findfall(true, headAngle);
timer.Start();
while (timer.Elapsed.Seconds < 25 && brain.lastPersonFallDetected.Count == 0)
{
;
}
timer.Stop();
//enviar comando para que deje de detectar caidas
cmdMan.VISION_findfall(false, 0);
if (brain.lastPersonFallDetected.Count > 0)
{
fallDetected = true;
}
else
{
fallDetected = false;
}
if (!fallDetected)
{
finalState = FinalStates.FallNotDetected;
return((int)States.FinalState);
}
return((int)States.GoToFall);
}
private int GoToDrinksLocation(int currentState, object o)
{
cmdMan.HEAD_lookat(0, 0, 10000);
TextBoxStreamWriter.DefaultLog.WriteLine("Going to drinks location.");
this.cmdMan.ARMS_goto("standby", 8000);
if (brain.GetCloseToTable(drinksLocation, 180000))
{
brain.SayAsync("i have arrived to the drinks location");
this.attemptCounter = 0;
return((int)States.SearchAndTakeObject);
}
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach drinks location, trying again");
return(currentState);
}
TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach drinks location, will try to continue with the test");
return((int)States.SearchAndTakeObject);
}
private int PerformHipnotize(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000-> PerformHypnotize state reached.");
t_SPGENHypnoSpeech = new Thread(new ThreadStart(SPGENHypnoSpeech));
t_ARMSHypnoMovements = new Thread(new ThreadStart(ARMSHypnoMovements));
t_HEADHypnoMovements = new Thread(new ThreadStart(HEADHypnoMovements));
//to allow threads execution
hypnoSpeechFinished = false;
//introduction to hypno show
cmdMan.SPG_GEN_shutup(1000);
this.brain.SayAsync("I will try to hipnotize you.");
Thread.Sleep(1000);
this.brain.SayAsync("Look straight to my pendulum.");
Thread.Sleep(1000);
//move both arms to navigation
moveActiveArms(SMConfiguration.ARMS_navigation, 10000);
//move left arm to hypno
moveOneArm(SMConfiguration.ARMS_hypnoc, 1, 10000);
//Launch the thread to move asynchronous the robot's arms (hypno left and right)
t_ARMSHypnoMovements.Start();
while (!t_ARMSHypnoMovements.IsAlive)
{
;
}
//Launch the threads to move asynchronous the robot's head ("circular" movement)
t_HEADHypnoMovements.Start();
while (!t_HEADHypnoMovements.IsAlive)
{
;
}
//Launch the thread to asynchronously play the speech
t_SPGENHypnoSpeech.Start();
while (!t_SPGENHypnoSpeech.IsAlive)
{
;
}
if (t_SPGENHypnoSpeech.IsAlive)
{
t_SPGENHypnoSpeech.Join();
}
if (t_HEADHypnoMovements.IsAlive)
{
t_HEADHypnoMovements.Join();
}
if (t_ARMSHypnoMovements.IsAlive)
{
t_ARMSHypnoMovements.Join();
}
//send the arms back to navigation and home
moveActiveArms(SMConfiguration.ARMS_navigation, 10000);
moveActiveArms(SMConfiguration.ARMS_home, 10000);
//look at the human face
cmdMan.HEAD_lookat(SMConfiguration.HEAD_lookToFace.pan, SMConfiguration.HEAD_lookToFace.tilt, 2000);
this.brain.SayAsync("now you are under my control!!!");
Thread.Sleep(1000);
this.brain.SayAsync("I command you to give me a token!");
Thread.Sleep(2000);
return((int)States.InitialState);
}
