private void RemoveObjectsInvolvingState(string stateName)
{
List <Instruction> temp = new List <Instruction>();
// Remove related instructions
foreach (Instruction instruction in Instructions)
{
if (instruction.CurrentState == stateName || instruction.NextState == stateName)
{
temp.Add(instruction);
}
}
foreach (Instruction instruction in temp)
{
Instructions.Remove(instruction);
}
// Remove related final states
if (FinalStates.Contains(stateName))
{
FinalStates.Remove(stateName);
}
// Remove related initial state
RemoveInitialStateInvolvingState(stateName);
}
private void ReduceStates()
{
var newState = 'A';
for (int i = 0; i < States.Count; i++)
{
if (StartState == States[i])
{
StartState = newState.ToString();
}
if (FinalStates.Contains(States[i]))
{
FinalStates[FinalStates.IndexOf(States[i])] = newState.ToString();
}
foreach (var tr in Transitions)
{
if (tr.CurrentState == States[i])
{
tr.CurrentState = newState.ToString();
}
if (tr.NextState == States[i])
{
tr.NextState = newState.ToString();
}
}
States[i] = newState.ToString();
newState++;
}
}
/// <summary>
/// Recursively checks to see if we can form the string
/// </summary>
/// <param name="state">the state to check</param>
/// <param name="remainingString">the remainder of the string to check</param>
/// <returns>A boolean that is true if the string has been fully formed</returns>
private bool CheckNextNodeDfa(T state, string stringToVerify)
{
bool stringIsAccepted = false;
if (stringToVerify == string.Empty) //Early escape if we're finished
{
stringIsAccepted = FinalStates.Contains(state);
return(stringIsAccepted);
}
char currentCharacter = stringToVerify[0];
if (stringToVerify.Count() > 1)
{
stringToVerify = stringToVerify.Substring(1);
}
else
{
stringToVerify = string.Empty;
}
IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => x.Identifier == currentCharacter && x.FromState.Equals(state));
foreach (Transition <T> transition in validTransitions)
{
stringIsAccepted = CheckNextNodeDfa(transition.ToState, stringToVerify);
if (stringIsAccepted)
{
break;
}
}
return(stringIsAccepted);
}
/// <summary>
/// Adds a letter to a language, then returns the string
/// </summary>
/// <param name="remainingLength">The remaining length for the string</param>
/// <param name="stringSoFar">The string generated so far.</param>
/// <param name="state">The state from which to generate a letter.</param>
/// <returns>Either returns a valid string, or an empty string if there is a non valid string.</returns>
private string AddLetterToLanguage(int remainingLength, string stringSoFar, T state)
{
string resultingString = string.Empty;
if (remainingLength > 0)
{
remainingLength = remainingLength - 1;
IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => x.FromState.Equals(state));
foreach (Transition <T> transition in validTransitions)
{
string newString = string.Concat(stringSoFar, transition.Identifier);
resultingString = AddLetterToLanguage(remainingLength, newString, transition.ToState);
if (string.IsNullOrEmpty(resultingString))
{
continue;
}
else
{
break;
}
}
}
else
{
resultingString = FinalStates.Contains(state) ? stringSoFar : string.Empty;
}
return(resultingString);
}
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, bool useHandOver)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
this.attemptCounter = 0;
this.searchAttemptCounter = 0;
this.finalState = FinalStates.StillRunning;
this.objectsFound = new List<string>();
this.useTakeHandOver = useHandOver;
this.anyObject = anyObject;
if (anyObject)
this.ObjectToFind = "objects";
else
this.ObjectToFind = ObjectToFind;
this.foundObject = ObjectToFind;
SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects)));
SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake)));
SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
public SSA <SYMBOL> Collapse(Func <SSA <SYMBOL>, int, int, bool> statesAreEquivalent)
{
/* map state to equivalency class representative */
var stateMap = new Dictionary <int, int>();
foreach (int state in States)
{
foreach (int repr in stateMap.Values.Distinct()) // search equivalency classes
{
if (statesAreEquivalent(this, state, repr))
{
stateMap[state] = repr; // add to equivalency class
break;
}
}
if (!stateMap.ContainsKey(state)) // equivalency class not found
{
stateMap[state] = state; // new equivalency class
}
}
int initialState = stateMap[InitialState];
var finalStates = new Set <int>(FinalStates.Select(fs => stateMap[fs]));
var moves = new Set <Move <Predicate <SYMBOL> > >(Moves.Select(move =>
new Move <Predicate <SYMBOL> >(stateMap[move.SourceState], stateMap[move.TargetState], move.Label)
));
var alphabet = new Set <SYMBOL>(Alphabet);
return(new SSA <SYMBOL>(initialState, finalStates, moves, alphabet));
}
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, bool useHandOver)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
this.attemptCounter = 0;
this.searchAttemptCounter = 0;
this.finalState = FinalStates.StillRunning;
this.objectsFound = new List <string>();
this.useTakeHandOver = useHandOver;
this.anyObject = anyObject;
if (anyObject)
{
this.ObjectToFind = "objects";
}
else
{
this.ObjectToFind = ObjectToFind;
}
this.foundObject = ObjectToFind;
SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects)));
SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake)));
SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
int SearchCloseObjects(int currentState, object o)
{
if (cmdMan.ST_PLN_fashionfind_object(ObjectToFind, 180000, out objectsFound))
{
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Objects found.");
return((int)States.SetObjectToTake);
}
if (attemptCounter < 2)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again");
this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again");
//this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return(currentState);
}
this.brain.SayAsync("I did not find " + (anyObject?"any":"") + ObjectToFind);
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful.");
attemptCounter = 0;
this.finalState = FinalStates.Failed;
return((int)States.FinalState);
}
public bool TestInput(string input)
{
Log += ConsoleWriter.Info("Trying to parse: " + input) + "\n";
if (InvalidInput(input))
{
return(false);
}
var currentState = StartState;
var steps = new StringBuilder();
foreach (var symbol in input.ToCharArray())
{
var transitions = TransitionFunctions;
var transition = transitions.Find(t => t.InputState == currentState &&
t.InputSymbol == symbol);
if (transition == null)
{
Log += ConsoleWriter.Failure("No transitions for current state and symbol") + "\n";
Log += ConsoleWriter.Failure(steps.ToString()) + "\n";
return(false);
}
currentState = transition.OutputState;
steps.Append(transition + "\n");
}
if (FinalStates.Contains(currentState))
{
Log += ConsoleWriter.Success("Accepted the input with steps:\n" + steps) + "\n";
return(true);
}
Log += ConsoleWriter.Failure("Stopped in state " + currentState +
" which is not a final state.") + "\n";
Log += ConsoleWriter.Failure(steps.ToString()) + "\n";
return(false);
}
public SM_TakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, string objectLocation, bool succesGetClose)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
this.objectLocation = objectLocation;
attemptCounter = 0;
this.finalState = FinalStates.StillRunning;
this.anyObject = anyObject;
if (anyObject)
this.ObjectToFind = ObjectToFind;
else
this.ObjectToFind = "objects";
this.foundObject = ObjectToFind;
this.succesGetClose = succesGetClose;
SM.AddState(new FunctionState((int)States.GetCloseObjectLocation, new SMStateFuncion(GetCloseObjectLocation)));
SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects)));
SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
private int SearchRequest(int currentState, object o)
{
bool gestureFound = false;
for (int i = 0; i < 18; ++i)
{
Thread.Sleep(500);
if (gestureFound = brain.FindSpecificGesture("request", out requestId))
{
cmdMan.MVN_PLN_fixhuman(requestId, 4000);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Gesture found.");
brain.SayAsync("I found a request.");
break;
}
}
if (!gestureFound)
{
if (attemptCounter > 7)
{
attemptCounter = 0;
finalState = FinalStates.RequestNotFound;
return((int)States.FinalState);
}
cmdMan.MVN_PLN_move(0, -Math.PI / 4, 4000);
if (attemptCounter % 2 == 1)
{
brain.SayAsync("I am looking for requests, please raise your hand above your head.");
}
attemptCounter++;
return(currentState);
}
// Si encontro humano, pasa al siguiente estado
attemptCounter = 0;
return((int)States.GoToRequest);
}
private int GoToFall(int currentState, object o)
{
bool notHumanFall;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> GoToFall state of SM_DetectPersonFall reached.");
if (brain.GetCloseToHumanFall(out notHumanFall, 10000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human success.");
finalState = FinalStates.OK;
return((int)States.FinalState);
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human FAILED");
if (!notHumanFall)
{
finalState = FinalStates.FallNotReached;
}
else
{
//brain.SayAsync("I have detected a fall in that direction.");
//point at the direction of the fall
//Thread.Sleep(1000);
finalState = FinalStates.NoHumanFallDetected;
}
return((int)States.FinalState);
}
}
public SM_TakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string ObjectToFind, string objectLocation, bool succesGetClose)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
this.objectLocation = objectLocation;
attemptCounter = 0;
this.finalState = FinalStates.StillRunning;
this.anyObject = anyObject;
if (anyObject)
{
this.ObjectToFind = ObjectToFind;
}
else
{
this.ObjectToFind = "objects";
}
this.foundObject = ObjectToFind;
this.succesGetClose = succesGetClose;
SM.AddState(new FunctionState((int)States.GetCloseObjectLocation, new SMStateFuncion(GetCloseObjectLocation)));
SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects)));
SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
private Grammar MakeGrammarLeft()
{
IDictionary <char, TerminalSymbol> charTerminalMap = Alphabet.ToDictionary(c => c, c => new TerminalSymbol(c));
IDictionary <Label, NonTerminalSymbol> stateNonTerminalMap = States.ToDictionary(s => s, s => new NonTerminalSymbol(s));
NonTerminalSymbol target = Grammar.GetNewNonTerminal(stateNonTerminalMap.Values);
ISet <Rule> rules = new HashSet <Rule>();
foreach (Transition transition in Transitions)
{
Chain chain = new Chain(
EnumerateHelper.Sequence <Symbol>(
stateNonTerminalMap[transition.CurrentState], charTerminalMap[transition.Symbol]
)
);
rules.Add(new Rule(chain.AsSequence(), stateNonTerminalMap[transition.NextState]));
}
rules.Add(new Rule(Chain.Empty.AsSequence(), stateNonTerminalMap[InitialState]));
IEnumerable <Chain> chains = FinalStates.Select(fs => new Chain(stateNonTerminalMap[fs].AsSequence()));
rules.Add(new Rule(chains, target));
return(new Grammar(rules, target));
}
public void RemoveState(ail.net.parser.FsaState xi_state)
{
ail.net.framework.Assert.NonNullReference(xi_state, "xi_state");
ail.net.framework.Assert.Condition(States.Contains(xi_state.Id), "States.Contains(xi_state.Id)");
FinalStates.Remove(xi_state.Id);
States.Remove(xi_state.Id);
}
/// <summary>
/// Add state to finalstates
/// </summary>
/// <param name="state">The state</param>
public void DefineAsFinalState(T state)
{
if (!DfaContainsState(state))
{
States.Add(state);
}
FinalStates.Add(state);
}
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);
}
public void DefineAsFinalState(T t)
{
if (!States.Contains(t))
{
States.Add(t);
}
FinalStates.Add(t);
}
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string [] objectsToFind, bool useHandOver, int searchAttempts = 2, bool twoArms = false)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
this.attemptCounter = 0;
this.searchAttemptCounter = 0;
this.searchAttempts = searchAttempts;
this.finalState = FinalStates.StillRunning;
this.twoArms = twoArms;
this.takeCounter = 0;
this.firstObjectTaken = false;
/**********************/
this.laTaken = false;
this.armToUse = "";
this.objectsFound = new List <string>();
this.useTakeHandOver = useHandOver;
this.objectsToFind = new string[2];
this.foundObjects = new string[2];
this.armsOrder = new string[2];
this.foundObjects[0] = "";
this.foundObjects[1] = "";
this.armsOrder[0] = "";
this.armsOrder[1] = "";
this.anyObject = anyObject;
if (anyObject)
{
//this.ObjectToFind = "objects";
this.objectsToFind[0] = "objects";
this.objectsToFind[1] = "";
}
else
{
this.objectsToFind[0] = objectsToFind[0];
this.objectsToFind[1] = objectsToFind[1];
}
//this.objectsToTake = new string[2];
//this.objectsToTake[0] = "";
//this.objectsToTake[1] = "";
//this.foundObjects = new string[2];
//this.foundObjects[0] = ObjectToFind;
//this.foundObjects[1] = "";
//objectsToTake = new string[2];
SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects)));
SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake)));
SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
public bool RemoveFinalState(string stateName)
{
if (FinalStates.Contains(stateName))
{
FinalStates.Remove(stateName);
return(true);
}
return(false);
}
public bool AddFinalState(string stateName)
{
if (States.Contains(stateName) && !FinalStates.Contains(stateName))
{
FinalStates.Add(stateName);
return(true);
}
return(false);
}
public SM_SearchAndTakeObject(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool anyObject, string [] objectsToFind, bool useHandOver, int searchAttempts = 2, bool twoArms=false)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
this.attemptCounter = 0;
this.searchAttemptCounter = 0;
this.searchAttempts = searchAttempts;
this.finalState = FinalStates.StillRunning;
this.twoArms = twoArms;
this.takeCounter = 0;
this.firstObjectTaken=false;
/**********************/
this.laTaken = false;
this.armToUse = "";
this.objectsFound = new List<string>();
this.useTakeHandOver = useHandOver;
this.objectsToFind = new string[2];
this.foundObjects = new string[2];
this.armsOrder = new string[2];
this.foundObjects[0] = "";
this.foundObjects[1] = "";
this.armsOrder[0] = "";
this.armsOrder[1] = "";
this.anyObject = anyObject;
if (anyObject)
{
//this.ObjectToFind = "objects";
this.objectsToFind[0] = "objects";
this.objectsToFind[1] = "";
}
else
{
this.objectsToFind[0] = objectsToFind[0];
this.objectsToFind[1] = objectsToFind[1];
}
//this.objectsToTake = new string[2];
//this.objectsToTake[0] = "";
//this.objectsToTake[1] = "";
//this.foundObjects = new string[2];
//this.foundObjects[0] = ObjectToFind;
//this.foundObjects[1] = "";
//objectsToTake = new string[2];
SM.AddState(new FunctionState((int)States.SearchCloseObjects, new SMStateFuncion(SearchCloseObjects)));
SM.AddState(new FunctionState((int)States.SetObjectToTake, new SMStateFuncion(SetObjectToTake)));
SM.AddState(new FunctionState((int)States.TakeObject, new SMStateFuncion(TakeObject)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
public bool IsWordInLanguage(String input, State startState)
{
//if we can finish now, finish now
if (input.Length == 0)
{
return(FinalStates.Contains(startState));
}
return(GetStatesAccessibleFrom(startState, input).Intersect(FinalStates).Count() > 0);
}
private int PersiguiendoPersona(int currentState, object o)
{
double distanc = Math.Sqrt(Math.Pow(hum.X, 2) + Math.Pow(hum.Y, 2));
if (distanc >= 0.5)
{
distanc = distanc - 0.10;
}
else if (distanc >= 0.3 && distanc < 0.5)
{
distanc = 0;
}
else if (distanc < 0.3)
{
distanc = -1;
}
double ang = 0;
if (hum.X == 0)
{
if (hum.Y > 0)
{
ang = Math.PI / 2;
}
else
{
ang = -Math.PI / 2;
}
}
else
{
ang = Math.Atan2(hum.Y, hum.X);
if (Math.Abs(ang) < AnguloEstable)
{
ang = 0;
}
}
if (distanc < 0)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> por fin");
finalState = FinalStates.HumanReached;
return((int)States.AntesDeTerminar);
}
else if (distanc == 0)
{
return((int)States.SensandoPersona);
}
else
{
this.cmdMan.MVN_PLN_move(distanc, ang);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> siguiendo a la persona");
return((int)States.SensandoPersona);
}
}
public void AddFinalState(ail.net.parser.FsaState xi_state, ail.net.parser.Token xi_token)
{
ail.net.framework.Assert.NonNullReference(xi_state, "xi_state");
ail.net.framework.Assert.NonNullReference(xi_token, "xi_token");
if (!FinalStates.Contains(xi_state.Id))
{
xi_state.Token = xi_token;
FinalStates.Add(xi_state.Id, xi_state);
}
}
private int TrainFace(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Sending <remember_human> command");
this.cmdMan.MVN_PLN_position(out x, out y, out angle, 2000);
string person;
if (this.cmdMan.ST_PLN_findhuman("human", "hdtilt hdpan", 90000, out person))
{
if (this.cmdMan.ST_PLN_rememberhuman(foundHuman, "", 60000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Face of " + foundHuman + " remembered succesfully");
brain.SayAsync("I have remembered your face.");
Thread.Sleep(500);
}
else
{
if (attemptCounter < 2)
{
attemptCounter++;
return(currentState);
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Cannot remember the face of " + foundHuman);
//brain.SayAsync("I cannot remember your face. I will continue with the test");
//return (int)States.TrainShirt;
this.finalState = FinalStates.HumanNotLearned;
return((int)States.FinalState);
}
}
}
else
{
if (attemptCounter < 2)
{
attemptCounter++;
return(currentState);
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Cannot find human.");
this.finalState = FinalStates.HumanNotFound;
//return (int)States.TrainShirt;
return((int)States.FinalState);
}
}
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumandRoutine.-> Desactivating reco human");
this.cmdMan.PRS_FND_sleep(true);
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
public KeyValuePair <bool, int> MaxString(string str, int pos)
{
KeyValuePair <bool, int> MaxStr = new KeyValuePair <bool, int>();
int m = 0;
bool res = false;
int curState = TakeNewStartState(str[pos]);
if (FinalStates.Contains(curState))
{
res = true;
}
int i = pos;
while (i < str.Length)
{
if (IsStateTransitionSignalsContainsSignal(curState, str[i]))
{
foreach (var state in States)
{
if (state.NameOfState == curState)
{
int newState = 0;
foreach (var list in state.StateTransitionSignals)
{
foreach (var elem in list)
{
if (elem == str[i])
{
curState = state.AvailableStates[newState];
}
}
newState++;
}
}
}
if (FinalStates.Contains(curState))
{
m = i - pos + 1;
res = true;
MaxStr = new KeyValuePair <bool, int>(res, m);
}
i++;
}
else
{
return(MaxStr);
}
}
return(MaxStr);
}
public StateMachine Reorganize(IDictionary <Label, Label> map)
{
if (map == null)
{
throw new ArgumentNullException(nameof(map));
}
Label initialState = map[InitialState];
IEnumerable <Label> finalStates = FinalStates.Select(fs => map[fs]);
IEnumerable <Transition> transitions = Transitions.Select(t => new Transition(map[t.CurrentState], t.Symbol, map[t.NextState]));
return(new StateMachine(initialState, finalStates, transitions));
}
/// <summary>
/// Recursively checks to see if we can form the string
/// </summary>
/// <param name="givenTransition">the transition to check</param>
/// <param name="remainingString">the remainder of the string to check</param>
/// <returns>A boolean that is true if the string has been fully formed</returns>
private bool CheckNextNodeNdfa(Transition <T> givenTransition, string stringToVerify)
{
bool stringIsAccepted = false;
if (stringToVerify == string.Empty) //Early escape if we're finished
{
stringIsAccepted = FinalStates.Contains(givenTransition.ToState);
if (stringIsAccepted)
{
return(stringIsAccepted);
}
IEnumerable <Transition <T> > possibleTransitionsLeadingToEndState = Transitions.Where(x => x.Identifier == '$' && x.FromState.Equals(givenTransition.ToState));
stringIsAccepted = CheckEmptyTransitions(possibleTransitionsLeadingToEndState);
return(stringIsAccepted);
}
char currentCharacter = stringToVerify[0];
if (stringToVerify.Count() > 1)
{
stringToVerify = stringToVerify.Substring(1);
}
else
{
stringToVerify = string.Empty;
}
IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => (x.Identifier == currentCharacter || x.Identifier == '$') && x.FromState.Equals(givenTransition.ToState));
foreach (Transition <T> transition in validTransitions)
{
if (transition.Identifier != '$')
{
stringIsAccepted = CheckNextNodeNdfa(transition, stringToVerify);
}
else
{
stringIsAccepted = CheckNextNodeNdfa(transition, currentCharacter + stringToVerify);
}
if (stringIsAccepted)
{
break;
}
}
return(stringIsAccepted);
}
int SetObjectToTake(int currentState, object o)
{
objectFoundIndex = 0;
this.foundObjectsCount = (byte)objectsFound.Count;
if (this.foundObjectsCount == 0)
{
if (!firstObjectTaken)
{
TextBoxStreamWriter.DefaultLog.WriteLine("No reachable objects were found, SM was not successful.");
this.finalState = FinalStates.Failed;
return((int)States.FinalState);
}
else
{
this.finalState = FinalStates.OK;
return((int)States.FinalState);
}
}
if (!anyObject)
{
if (objectsFound.Contains(objectsToFind[takeCounter]))
{
objectFoundIndex = objectsFound.IndexOf(objectsToFind[takeCounter]);
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object found.");
return((int)States.TakeObject);
}
///******28-05-2013*******////////////
if (searchAttemptCounter < searchAttempts)
{
searchAttemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("object NOT found, trying again");
this.brain.SayAsync("I did not find the " + objectsToFind[takeCounter] + ". I will try again");
//this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return((int)States.SearchCloseObjects);
}
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, test failed.");
this.finalState = FinalStates.Failed;
return((int)States.FinalState);
}
objectsToFind[takeCounter] = objectsFound[0];
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Some Object found.");
return((int)States.TakeObject);
}
/// <summary>
/// Checks whether the given string fits within this automata
/// </summary>
/// <param name="stringToVerify">The string to check.</param>
/// <returns>A boolean indicator that is true if the string can be formed by this automata by traversing it's nodes.</returns>
private bool IsStringAcceptableNdfa(string stringToVerify)
{
bool stringIsAccepted = false;
//Pak start states + eerste element in char array
char charToCheckFor = stringToVerify[0];
HashSet <T> startingStates = StartStates;
//Verwijder eerste character
if (stringToVerify.Count() > 1)
{
stringToVerify = stringToVerify.Substring(1);
}
else
{
stringToVerify = string.Empty;
}
//Voor iedere uitgaande transitie check of het kan met het gegeven character
foreach (T state in startingStates)
{
if (stringToVerify == string.Empty)
{
stringIsAccepted = FinalStates.Contains(state);
break;
}
IEnumerable <Transition <T> > validTransitions = Transitions.Where(x => (x.Identifier == charToCheckFor || x.Identifier == '$') && x.FromState.Equals(state));
foreach (Transition <T> transition in validTransitions)
{
if (transition.Identifier != '$')
{
stringIsAccepted = CheckNextNodeNdfa(transition, stringToVerify);
}
else
{
stringIsAccepted = CheckNextNodeNdfa(transition, charToCheckFor + stringToVerify);
}
if (stringIsAccepted)
{
break;
}
}
}
//Als er een terug komt met true, dan bestaat de string
return(stringIsAccepted);
}
/// <summary>
/// Creates a state machine to enter the arena up to a location known by the motion planner.
/// </summary>
/// <param name="brain">A HAL9000Brain instance</param>
/// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param>
public SM_WaitForRequest(HAL9000Brain brain, HAL9000CmdMan cmdMan)
{
this.brain = brain;
this.cmdMan = cmdMan;
this.attemptCounter = 0;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState)));
SM.AddState(new FunctionState((int)States.SearchRequest, new SMStateFuncion(SearchRequest)));
SM.AddState(new FunctionState((int)States.GoToRequest, new SMStateFuncion(GoToRequest)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true));
SM.SetFinalState((int)States.FinalState);
finalState = FinalStates.StillRunning;
}
/// <summary>
/// Creates a state machine to enter the arena up to a location known by the motion planner.
/// </summary>
/// <param name="brain">A HAL9000Brain instance</param>
/// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param>
public SM_WaitForRequest(HAL9000Brain brain, HAL9000CmdMan cmdMan)
{
this.brain = brain;
this.cmdMan = cmdMan;
this.attemptCounter = 0;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState)));
SM.AddState(new FunctionState((int)States.SearchRequest, new SMStateFuncion(SearchRequest)));
SM.AddState(new FunctionState((int)States.GoToRequest, new SMStateFuncion(GoToRequest)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true));
SM.SetFinalState((int)States.FinalState);
finalState = FinalStates.StillRunning;
}
private int SearchWaving(int currentState, object o)
{
double xFall, zFall;
/*send command to find waving*/
//emergencyFound = false;
//if (!emergencyFound)
Thread.Sleep(1000);
if (!cmdMan.VISION_findwaving(headAngle, out xFall, out zFall, 10000))
{
finalState = FinalStates.WavingNotFound;
}
else
{
double distance, angle;
//double robotX, robotY, robotAngle;
//double goalX, goalY;
//this.cmdMan.MVN_PLN_position(out robotX, out robotY, out robotAngle, 1000);
distance = ((Math.Sqrt(Math.Pow(xFall, 2) + Math.Pow(zFall, 2))));
angle = Math.Atan2(-xFall, zFall);
//angle = 1.5708-robotAngle;
//goalX = robotX + (xFall * Math.Cos(angle) - zFall * Math.Sin(angle));
//goalY = robotY + (xFall * Math.Sin(angle) + zFall * Math.Cos(angle));
/*if(!cmdMan.MVN_PLN_getclose(goalX,goalY,10000))
* if (!cmdMan.MVN_PLN_getclose(goalX, goalY, 10000))
* cmdMan.MVN_PLN_getclose(goalX, goalY, 10000);
*/
if (!cmdMan.MVN_PLN_move((3 * distance) / 5, angle + headPan, 10000))
{
if (!cmdMan.MVN_PLN_move((3 * distance) / 5, angle + headPan, 10000))
{
cmdMan.MVN_PLN_move((3 * distance) / 5, angle + headPan, 10000);
}
}
//getclose to waving
finalState = FinalStates.OK;
}
return((int)States.FinalState);
}
/// <summary>
/// Creates a state machine to enter the arena up to a location known by the motion planner.
/// </summary>
/// <param name="brain">A HAL9000Brain instance</param>
/// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param>
/// <param name="tryToOpenDoor">A boolean indicating whether the robot should try to open the door on its own or ask for help directly.</param>
public SM_EnterArena(HAL9000Brain brain, HAL9000CmdMan cmdMan, string location, bool tryToOpenDoor)
{
this.brain = brain;
this.cmdMan = cmdMan;
this.location = location;
this.finalState = FinalStates.StillRunning;
this.attemptCounter = 0;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState)));
SM.AddState(new FunctionState((int)States.WaitForDoorToBeOpened, new SMStateFuncion(WaitForDoorToBeOpened)));
SM.AddState(new FunctionState((int)States.EnterArena, new SMStateFuncion(EnterArena)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true));
SM.SetFinalState((int)States.FinalState);
}
/// <summary>
/// Creates a state machine to enter the arena up to a location known by the motion planner.
/// </summary>
/// <param name="brain">A HAL9000Brain instance</param>
/// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param>
public SM_DetectPersonFall(HAL9000Brain brain, HAL9000CmdMan cmdMan)
{
this.brain = brain;
this.cmdMan = cmdMan;
timer = new System.Diagnostics.Stopwatch();
fallDetected = false;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState)));
SM.AddState(new FunctionState((int)States.SearchFall, new SMStateFuncion(SearchFall)));
SM.AddState(new FunctionState((int)States.GoToFall, new SMStateFuncion(GoToFall)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true));
SM.SetFinalState((int)States.FinalState);
finalState = FinalStates.StillRunning;
}
/// <summary>
/// Creates a state machine to enter the arena up to a location known by the motion planner.
/// </summary>
/// <param name="brain">A HAL9000Brain instance</param>
/// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param>
public SM_DetectWaving(HAL9000Brain brain, HAL9000CmdMan cmdMan, double headAngle, double headPan)
{
this.brain = brain;
this.cmdMan = cmdMan;
this.headAngle = headAngle;
this.headPan = headPan;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState)));
SM.AddState(new FunctionState((int)States.SearchWaving, new SMStateFuncion(SearchWaving)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true));
SM.SetFinalState((int)States.FinalState);
finalState = FinalStates.StillRunning;
}
public SM_FollowHuman(HAL9000Brain brain, HAL9000CmdMan cmdman)
{
this.brain = brain;
this.cmdMan = cmdman;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Ejecutando prueba de seguimiento");
this.brain.Status.TestBeingExecuted = "Fllw1";
this.brain.OnStatusChanged(new HAL9000StatusArgs(this.brain.Status));
this.finalState = FinalStates.StillRunning;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.SubiendoMisBrazos, SubiendoMisBrazos));
SM.AddState(new FunctionState((int)States.EsperandoPersonaEnFrente, EsperandoPersonaEnFrente));
SM.AddState(new FunctionState((int)States.SensandoPersona, SensandoPersona));
SM.AddState(new FunctionState((int)States.PersiguiendoPersona, PersiguiendoPersona));
SM.AddState(new FunctionState((int)States.AntesDeTerminar, AntesDeTerminar, true));
SM.AddState(new FunctionState((int)States.FinalState, FinalState, true));
SM.SetFinalState((int)States.FinalState);
}
public SM_TakeTray(HAL9000Brain brain, HAL9000CmdMan cmdMan, double height)
{
SM = new FunctionBasedStateMachine();
this.brain = brain;
this.cmdMan = cmdMan;
distance = 0;
this.height = height;
headPan = 0;
headTilt = -1.1;
percent = 50;
this.finalState = FinalStates.StillRunning;
SM.AddState(new FunctionState((int)States.LineTable,LineTable));
SM.AddState(new FunctionState((int)States.TakeTray, TakeTray));
SM.AddState(new FunctionState((int)States.FinalState, FinalState,true));
SM.SetFinalState((int)States.FinalState);
}
/// <summary>
/// Creates a state machine to enter the arena up to a location known by the motion planner.
/// </summary>
/// <param name="brain">A HAL9000Brain instance</param>
/// <param name="location">A location known by the motion planner where the robot should try to go when entering the arena.</param>
public SM_SimpleIdentifyPerson(HAL9000Brain brain, HAL9000CmdMan cmdMan, bool startIdentifyPerson, string[] knownPersons, string defaultName, List<string> rejectedNames)
{
this.brain = brain;
this.cmdMan = cmdMan;
this.startIdentifyPerson = startIdentifyPerson;
this.defaultName = defaultName;
this.knownPersons = knownPersons;
this.rejectedNames = rejectedNames;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.InitialState, new SMStateFuncion(InitialState)));
SM.AddState(new FunctionState((int)States.SpeachToHuman, new SMStateFuncion(SpeachToHuman)));
SM.AddState(new FunctionState((int)States.FindHuman, new SMStateFuncion(FindHuman)));
SM.AddState(new FunctionState((int)States.AskForName, new SMStateFuncion(AskForName)));
SM.AddState(new FunctionState((int)States.AsociateName, new SMStateFuncion(AsociateName)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState), true));
SM.SetFinalState((int)States.FinalState);
finalState = FinalStates.StillRunning;
}
public CocktailPartyTest(HAL9000Brain brain, HAL9000CmdMan cmdMan)
{
this.brain = brain;
this.cmdMan = cmdMan;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Executing Cocktail Party Test");
this.brain.Status.TestBeingExecuted = "Cocktail Party";
this.brain.OnStatusChanged(new HAL9000StatusArgs(this.brain.Status));
this.finalState = FinalStates.StillRunning;
tryToOpenDoor = false;
entranceLocation = "frontentrance";
partyroomLocation = "partyroom";
drinksLocation = "drinks";
exitLocation = "exit";
defaultDrink = "mangojuice";
attemptCounter = 0;
gestureFailed = false;
defaultNames = new string[] { "Alan", "Albert", "Angel" };
SM = new FunctionBasedStateMachine();
//orders = new Queue<order>(3);
ordersCount = 0;
rejectedNames = new List<string>(3);
SM.AddState(new FunctionState((int)States.EnterArena, new SMStateFuncion(EnterArena)));
SM.AddState(new FunctionState((int)States.GoToPartyRoom, new SMStateFuncion(GoToPartyRoom)));
SM.AddState(new FunctionState((int)States.WaitForRequest, new SMStateFuncion(WaitForRequest)));
SM.AddState(new FunctionState((int)States.IdentifyPerson, new SMStateFuncion(IdentifyPerson)));
SM.AddState(new FunctionState((int)States.WaitForOrder, new SMStateFuncion(WaitForOrder)));
SM.AddState(new FunctionState((int)States.NoGestureRecognized, new SMStateFuncion(NoGestureRecognized)));
SM.AddState(new FunctionState((int)States.WaitForConfirmation, new SMStateFuncion(WaitForConfirmation)));
SM.AddState(new FunctionState((int)States.EnqueueOrder, new SMStateFuncion(EnqueueOrder)));
SM.AddState(new FunctionState((int)States.GoToDrinksLocation, new SMStateFuncion(GoToDrinksLocation)));
SM.AddState(new FunctionState((int)States.SearchAndTakeObject, new SMStateFuncion(SearchAndTakeObject)));
SM.AddState(new FunctionState((int)States.GoToHumanLocation, new SMStateFuncion(GoToHumanLocation)));
SM.AddState(new FunctionState((int)States.DeliverDrink, new SMStateFuncion(DeliverDrink)));
SM.AddState(new FunctionState((int)States.LeaveArena, new SMStateFuncion(LeaveArena)));
SM.AddState(new FunctionState((int)States.FinalState, new SMStateFuncion(FinalState)));
SM.SetFinalState((int)States.FinalState);
}
public OpenChallenge_Training(HAL9000Brain brain, HAL9000CmdMan cmdMan)
{
this.brain = brain;
this.cmdMan = cmdMan;
this.brain.UseFloorLaser = false;
height = 0;
LocationTable = "table";
LocationTray = "tray";
percent = 50;
distance = 0;
headPan = 0;
headTilt = -1.1;
voiceRecognitionAttempts = 0;
alreadyKnownPersons = new List<string>();
defaultNames = new string[] {"John", "James", "William", "Michael"};
personCounter = 0;
usingRightArm = true;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Executing Open Challenge");
this.brain.Status.TestBeingExecuted = "Open Challenge";
this.brain.OnStatusChanged(new HAL9000StatusArgs(this.brain.Status));
this.finalState = FinalStates.StillRunning;
SM = new FunctionBasedStateMachine();
SM.AddState(new FunctionState((int)States.CallRobot, CallRobot));
SM.AddState(new FunctionState((int)States.TrainFace, TrainFace));
SM.AddState(new FunctionState((int)States.DeliverSheet, DeliverSheet));
SM.AddState(new FunctionState((int)States.TrainVoice, TrainVoice));
SM.AddState(new FunctionState((int)States.ReceiveSheet, ReceiveSheet));
SM.AddState(new FunctionState((int)States.AdjustTable, AdjustTable));
SM.AddState(new FunctionState((int)States.GotoTray, GotoTray));
SM.AddState(new FunctionState((int)States.TakeTray, TakeTray));
SM.AddState(new FunctionState((int)States.GotoTable, GotoTable));
SM.AddState(new FunctionState((int)States.TrayOnTable, TrayOnTable));
SM.AddState(new FunctionState((int)States.FinalState, FinalState, true));
SM.SetFinalState((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 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 GoToFall(int currentState, object o)
{
bool notHumanFall;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> GoToFall state of SM_DetectPersonFall reached.");
if(brain.GetCloseToHumanFall(out notHumanFall, 10000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human success.");
finalState = FinalStates.OK;
return (int)States.FinalState;
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human FAILED");
if (!notHumanFall)
finalState = FinalStates.FallNotReached;
else
{
//brain.SayAsync("I have detected a fall in that direction.");
//point at the direction of the fall
//Thread.Sleep(1000);
finalState = FinalStates.NoHumanFallDetected;
}
return (int)States.FinalState;
}
}
private int SearchRequest(int currentState, object o)
{
bool gestureFound = false;
for (int i = 0; i < 18; ++i)
{
Thread.Sleep(500);
if (gestureFound = brain.FindSpecificGesture("request", out requestId))
{
cmdMan.MVN_PLN_fixhuman(requestId, 4000);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Gesture found.");
brain.SayAsync("I found a request.");
break;
}
}
if (!gestureFound)
{
if (attemptCounter > 7)
{
attemptCounter = 0;
finalState = FinalStates.RequestNotFound;
return (int)States.FinalState;
}
cmdMan.MVN_PLN_move(0, -Math.PI / 4, 4000);
if (attemptCounter % 2 == 1)
brain.SayAsync("I am looking for requests, please raise your hand above your head.");
attemptCounter++;
return currentState;
}
// Si encontro humano, pasa al siguiente estado
attemptCounter = 0;
return (int)States.GoToRequest;
}
private int EnterArena(int currentState, object o)
{
this.cmdMan.ARMS_goto("standby", 8000);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Trying getclose to: " + location);
if (this.cmdMan.MVN_PLN_getclose(location, 300000))
{
this.finalState = FinalStates.OK;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Entered the arena");
return (int)States.FinalState;
}
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Cannot arrive to entrance location, trying again.");
return (int)States.EnterArena;
}
this.finalState = FinalStates.Failed;
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Cannot arrive to entrance location, SM was NOT successful.");
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;
}
private int GoToRequest(int currentState, object o)
{
if (attemptCounter < 3)
{
if (cmdMan.MVN_PLN_getclose("human " + requestId, 60000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human success at attempt" + attemptCounter);
finalState = FinalStates.OK;
return (int)States.FinalState;
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> Getclose to human FAILED at attempt" + attemptCounter);
attemptCounter++;
return currentState;
}
}
attemptCounter = 0;
finalState = FinalStates.HumanNotReachable;
return (int)States.FinalState;
}
int SetObjectToTake(int currentState, object o)
{
objectFoundIndex = 0;
this.foundObjectsCount = (byte)objectsFound.Count;
if (this.foundObjectsCount == 0)
{
if (!firstObjectTaken)
{
TextBoxStreamWriter.DefaultLog.WriteLine("No reachable objects were found, SM was not successful.");
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
}
else
{
this.finalState = FinalStates.OK;
return (int)States.FinalState;
}
}
if (!anyObject)
{
if (objectsFound.Contains(objectsToFind[takeCounter]))
{
objectFoundIndex = objectsFound.IndexOf(objectsToFind[takeCounter]);
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object found.");
return (int)States.TakeObject;
}
///******28-05-2013*******////////////
if (searchAttemptCounter < searchAttempts )
{
searchAttemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("object NOT found, trying again");
this.brain.SayAsync("I did not find the " + objectsToFind[takeCounter] + ". I will try again");
//this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return (int)States.SearchCloseObjects;
}
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, test failed.");
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
}
objectsToFind[takeCounter] = objectsFound[0];
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Some Object found.");
return (int)States.TakeObject;
}
private int PersiguiendoPersona(int currentState, object o)
{
double distanc =Math.Sqrt(Math.Pow(hum.X, 2) + Math.Pow(hum.Y, 2));
if (distanc >= 0.5)
{
distanc = distanc-0.10;
}
else if (distanc >= 0.3 && distanc < 0.5)
{
distanc = 0;
}
else if (distanc<0.3)
{
distanc = -1;
}
double ang = 0;
if (hum.X == 0)
{
if (hum.Y > 0)
{
ang = Math.PI / 2;
}
else
{
ang = -Math.PI / 2;
}
}
else
{
ang = Math.Atan2(hum.Y, hum.X);
if (Math.Abs(ang) < AnguloEstable)
{
ang = 0;
}
}
if (distanc < 0)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> por fin");
finalState = FinalStates.HumanReached;
return (int)States.AntesDeTerminar;
}
else if(distanc==0)
{
return (int)States.SensandoPersona;
}
else
{
this.cmdMan.MVN_PLN_move(distanc, ang);
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000.-> siguiendo a la persona");
return (int)States.SensandoPersona;
}
}
int SearchCloseObjects(int currentState, object o)
{
List<string> objectsFound = new List<string>();
if (cmdMan.ST_PLN_fashionfind_object(ObjectToFind, 90000, out objectsFound))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Objects found.");
this.foundObjectsCount = (byte)objectsFound.Count;
if (!anyObject)
{
if (objectsFound.Contains(ObjectToFind))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object found.");
attemptCounter = 0;
return (int)States.TakeObject;
}
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("object NOT found, trying again");
this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again");
//this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return currentState;
}
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, will try to continue the test.");
attemptCounter = 0;
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
/*foreach (string obj in objectsFound)
{
//cmdMan.SPG_GEN_say(obj);
if (obj == objectToBring)
{
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object founded");
nextStage = 70;
founded = true;
}
}*/
}
else
{
ObjectToFind = objectsFound[0];
foundObject = objectsFound[0];
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Some Object found.");
return (int)States.TakeObject;
}
}
else
{
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again");
this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again");
this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return currentState;
}
this.brain.SayAsync("I did not find " + (anyObject?"any":"") + ObjectToFind + " in " + objectLocation);
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful.");
attemptCounter = 0;
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
}
}
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 SearchWaving(int currentState, object o)
{
double xFall, zFall;
/*send command to find waving*/
//emergencyFound = false;
//if (!emergencyFound)
Thread.Sleep(1000);
if (!cmdMan.VISION_findwaving(headAngle, out xFall, out zFall, 10000))
finalState = FinalStates.WavingNotFound;
else
{
double distance, angle;
//double robotX, robotY, robotAngle;
//double goalX, goalY;
//this.cmdMan.MVN_PLN_position(out robotX, out robotY, out robotAngle, 1000);
distance = ((Math.Sqrt(Math.Pow(xFall, 2) + Math.Pow(zFall, 2))));
angle = Math.Atan2(-xFall, zFall);
//angle = 1.5708-robotAngle;
//goalX = robotX + (xFall * Math.Cos(angle) - zFall * Math.Sin(angle));
//goalY = robotY + (xFall * Math.Sin(angle) + zFall * Math.Cos(angle));
/*if(!cmdMan.MVN_PLN_getclose(goalX,goalY,10000))
if (!cmdMan.MVN_PLN_getclose(goalX, goalY, 10000))
cmdMan.MVN_PLN_getclose(goalX, goalY, 10000);
*/
if (!cmdMan.MVN_PLN_move((3*distance)/5, angle+headPan, 10000))
if (!cmdMan.MVN_PLN_move((3*distance)/5, angle + headPan, 10000))
cmdMan.MVN_PLN_move((3*distance)/5, angle + headPan, 10000);
//getclose to waving
finalState = FinalStates.OK;
}
return (int)States.FinalState;
}
private int AsociateName(int currentState, object o)
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Confirm \"yes\" received");
brain.SayAsync("O.K. I am going to remember your face. Please look straight forward to my eyes");
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Sending <remember_human> command");
if (this.cmdMan.ST_PLN_rememberhuman(foundHuman, "", 60000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Face of " + foundHuman + " remembered succesfully");
this.cmdMan.SPG_GEN_say("I have remembered your face.", 10000);
this.cmdMan.HEAD_lookat(0, 0, 4000);
this.cmdMan.MVN_PLN_position(out x, out y, out angle, 2000);
}
else
{
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumanRoutine.-> Cannot remember the face of " + foundHuman);
this.cmdMan.SPG_GEN_say("I cannot remember your face. I will continue with the test", 12000);
this.cmdMan.HEAD_lookat(0, 0, 4000);
}
TextBoxStreamWriter.DefaultLog.WriteLine("HAL9000\\FindHumandRoutine.-> Desactivating reco human");
this.cmdMan.PRS_FND_sleep(true);
this.finalState = FinalStates.OK;
return (int)States.FinalState;
}
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 SearchCloseObjects(int currentState, object o)
{
if (cmdMan.ST_PLN_fashionfind_object(objectsToFind[takeCounter], 270000, out objectsFound))
{
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Objects found.");
this.totalFoundObjects = (byte)objectsFound.Count;
return (int)States.SetObjectToTake;
}
/***28-05-2013*////
if (attemptCounter < searchAttempts)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again");
this.brain.SayAsync("I did not find the " + objectsToFind[takeCounter] + ". I will try again");
//this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return currentState;
}
this.brain.SayAsync("I did not find " + (anyObject ? "any " : "the ") + objectsToFind[takeCounter]);
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful.");
attemptCounter = 0;
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
}
private int LeaveArena(int currentState, object o)
{
this.cmdMan.ARMS_goto("standby", 8000);
if (cmdMan.MVN_PLN_getclose(exitLocation, 120000))
{
TextBoxStreamWriter.DefaultLog.WriteLine("Location reached: " + exitLocation);
this.finalState = FinalStates.OK;
return (int)States.FinalState;
}
if (attemptCounter < 3)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach location: " + exitLocation + ", trying again");
return currentState;
}
TextBoxStreamWriter.DefaultLog.WriteLine("Cant reach location: " + exitLocation + ", SM execution was NOT successful");
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
}
int SearchCloseObjects(int currentState, object o)
{
if (cmdMan.ST_PLN_fashionfind_object(ObjectToFind, 180000, out objectsFound))
{
attemptCounter = 0;
TextBoxStreamWriter.DefaultLog.WriteLine("Objects found.");
return (int)States.SetObjectToTake;
}
if (attemptCounter < 2)
{
attemptCounter++;
TextBoxStreamWriter.DefaultLog.WriteLine("No objects were found, trying again");
this.brain.SayAsync("I did not find the " + ObjectToFind + ". I will try again");
//this.cmdMan.MVN_PLN_move(-0.25, 0, 0, 5000);
Thread.Sleep(2500);
return currentState;
}
this.brain.SayAsync("I did not find " + (anyObject?"any":"") + ObjectToFind);
TextBoxStreamWriter.DefaultLog.WriteLine("Requested Object NOT found, SM was not successful.");
attemptCounter = 0;
this.finalState = FinalStates.Failed;
return (int)States.FinalState;
}
