protected override Response AsyncTask(Command command)
{
TBWriter.Info1("----------------------------------------------");
TBWriter.Spaced(" >>>>>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string[] info = command.Parameters.Split(null as char[], StringSplitOptions.RemoveEmptyEntries);
if (info.Length < 2)
{
TBWriter.Error("Cant execute CmdExecuteLearn, invalid parameters");
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
TBWriter.Info1("----------------------------------------------");
return(Response.CreateFromCommand(command, false));
}
bool success = this.taskPlanner.Cmd_ExecuteLearn(info[0], info[1]);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
TBWriter.Info1("----------------------------------------------");
return(Response.CreateFromCommand(command, success));
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string obj1 = "";
string obj2 = "";
string[] parameters = command.Parameters.Split(null as char[], StringSplitOptions.RemoveEmptyEntries);
if (parameters.Length < 1)
{
TBWriter.Error("Invalid parmeters");
return(Response.CreateFromCommand(command, false));
}
if (parameters.Length == 1)
{
obj1 = parameters[0];
}
if (parameters.Length == 2)
{
obj1 = parameters[0];
obj2 = parameters[1];
}
bool success = taskPlanner.Cmd_TakeHandOver(obj1, obj2);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, success));
}
protected override Response AsyncTask(Command command)
{
TBWriter.Info1("----------------------------------------------");
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
bool succes;
WorldObject[] objectsFounded;
if (!taskPlanner.Cmd_FindObject(out objectsFounded))
{
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
TBWriter.Info1("----------------------------------------------");
return(Response.CreateFromCommand(command, false));
}
if (objectsFounded == null)
{
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
TBWriter.Info1("----------------------------------------------");
return(Response.CreateFromCommand(command, false));
}
string objectsList = "";
foreach (WorldObject obj in objectsFounded)
{
objectsList += obj.Name + " ";
}
command.Parameters = objectsList;
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
TBWriter.Info1("----------------------------------------------");
return(Response.CreateFromCommand(command, true));
}
protected override Response SyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string humanName;
Vector3 posiblePosition;
double humanPan;
double humanTilt;
if (Parse.humanInfo(command.Parameters, out humanName, out posiblePosition, out humanPan, out humanTilt))
{
posiblePosition = taskPlanner.robot.TransMinoru2Robot(posiblePosition);
taskPlanner.personPFAutoDetected = new Human(humanName, posiblePosition);
TBWriter.Info2("PFAutoDetected : founded " + humanName + " located in " + posiblePosition.ToString());
this.cmdMan.PRS_FND_auto(false, "");
}
else
{
taskPlanner.personPFAutoDetected = null;
}
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(null);
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string[] info = command.Parameters.Split(null as char[], StringSplitOptions.RemoveEmptyEntries);
if ((info == null) || (info.Length == 0))
{
return(Response.CreateFromCommand(command, false));
}
string obj1 = "";
string obj2 = "";
string armSelection = "";
string objectsTaked = "";
obj1 = info[0];
if ((info.Length > 1) && (info[1] != "left") && (info[1] != "right"))
{
obj2 = info[1];
}
else
{
obj2 = "";
}
if (command.Parameters.Contains("right"))
{
armSelection = "right";
}
else if (command.Parameters.Contains("left"))
{
armSelection = "left";
}
else
{
armSelection = "";
}
if (taskPlanner.Cmd_TakeObjFromShelf(obj1, obj2, armSelection, out objectsTaked))
{
command.Parameters = objectsTaked;
return(Response.CreateFromCommand(command, true));
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
}
else
{
command.Parameters = objectsTaked;
return(Response.CreateFromCommand(command, false));
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
}
}
protected override Response SyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
this.taskPlanner.Cmd_StopLearn();
return(Response.CreateFromCommand(command, true));
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
}
void cnnMan_ClientConnected(System.Net.Sockets.Socket s)
{
connectedToBlackboard = true;
TBWriter.Spaced(TBWriter.time() + " Connected to Blackboard ");
sharedVarSubscriptionThread = new Thread(new ThreadStart(sharedVarSubscriptionThreadTask));
sharedVarSubscriptionThread.IsBackground = true;
sharedVarSubscriptionThread.Start();
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
bool success = taskPlanner.Cmd_Drop(command.Parameters);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, success));
}
protected override Response AsyncTask(Command command)
{
bool result = false;
TBWriter.Spaced(TBWriter.time() + " ===>>> COMMAND RECEIVED : " + command.CommandName);
result = taskPlanner.Cmd_TakeTable();
TBWriter.Spaced(TBWriter.time() + " <<<=== COMMAND TERMINATED : " + command.CommandName + " ; Result{" + result + "}");
return(Response.CreateFromCommand(command, result));
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
if (string.IsNullOrEmpty(command.Parameters))
{
TBWriter.Error("Command Parameters is null or empty");
return(Response.CreateFromCommand(command, false));
}
bool success = taskPlanner.Cmd_DeliverObject(command.Parameters);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, success));
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string holdinginfo = "";
if (!this.taskPlanner.Cmd_Holding(out holdinginfo))
{
return(Response.CreateFromCommand(command, false));
}
command.Parameters = holdinginfo;
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, true));
}
protected override Response AsyncTask(Command command)
{
bool result;
double[] desireParams;
TBWriter.Spaced(TBWriter.time() + " ===>>> COMMAND RECEIVED : " + command.CommandName + " ; Params{" + command.Parameters + "}");
if (!Parse.string2doubleArray(command.Parameters, out desireParams))
{
desireParams = new double[2];
desireParams[0] = 0.0;
desireParams[1] = 0.15;
}
result = taskPlanner.Cmd_AlignWithEdge(desireParams[0], desireParams[1]);
TBWriter.Spaced(TBWriter.time() + " <<<=== COMMAND TERMINATED : " + command.CommandName + " ; Result{" + result + "} Params{" + command.Parameters + "}");
return(Response.CreateFromCommand(command, result));
}
public STPlanner()
{
TBWriter.Spaced("Simple Task Planner, Version 0.98798");
TextBoxStreamWriter.DefaultLog.WriteLine(0, "asdasd as asd ");
TextBoxStreamWriter.DefaultLog.WriteLine(0, "Builded 16/feb/13 , 15:351 p.m.");
TextBoxStreamWriter.DefaultLog.WriteLine(0, "");
cmdMan = new STPCommandManager();
cnnMan = new ConnectionManager(connectionPort, this.cmdMan);
TBWriter.Write(" Connection Port = " + connectionPort.ToString());
this.cnnMan.ClientConnected += new System.Net.Sockets.TcpClientConnectedEventHandler(cnnMan_ClientConnected);
TBWriter.Spaced(TBWriter.time() + " Waiting for Blackboard Connection . . .");
taskPlanner = new TaskPlanner(cmdMan, this);
LoadCommands();
sharedVarConnected = new StringSharedVariable("connected");
sharedVarSkeletons = new StringSharedVariable("skeletons");
sharedVarOdometryPos = new DoubleArraySharedVariable("odometryPos");
sharedVarHdPos = new DoubleArraySharedVariable("hd_pos");
sharedVarTorso = new DoubleArraySharedVariable("torsoPosition");
sharedVarRobotRegion = new StringSharedVariable("robotRoom");
//cmdMan.SharedVariablesLoaded += new SharedVariablesLoadedEventHandler(cmdMan_SharedVariablesLoaded);
cnnMan.Start();
TBWriter.Write(3, " > Connection Manager Started");
cmdMan.Start();
TBWriter.Write(3, " > Command Manager Started");
//if(!sharedVarLoadEvent) TBWriter.Spaced(" Waiting for Shared Variables Load from BlackBoard . . .")
}
protected override Response SyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string[] cmdinfo = command.Parameters.Split(null as char[], StringSplitOptions.RemoveEmptyEntries);
if (cmdinfo.Length < 1)
{
TBWriter.Error("Invalid parameters ");
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, false));
}
string motionLearningName = cmdinfo[0];
this.taskPlanner.Cmd_StartLearn(motionLearningName);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, true));
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
Human personFounded;
string[] param = command.Parameters.Split(null as char[], StringSplitOptions.RemoveEmptyEntries);
string humanName;
string devices;
if (param == null)
{
humanName = "human";
devices = "";
}
else if (param.Length == 1)
{
humanName = param[0];
devices = "";
}
else
{
humanName = param[0];
devices = command.Parameters;
}
bool success = taskPlanner.Cmd_FindHuman(humanName, devices, out personFounded);
if (success)
{
command.Parameters = personFounded.Name;
}
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, success));
}
protected override Response SyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
bool autoLocalization;
//if (command.Parameters == "enable")
//{
// taskPlanner.AutoLocalization = true;
// return Response.CreateFromCommand(command, taskPlanner.AutoLocalization);
//}
//if (command.Parameters == "disable")
//{
// taskPlanner.AutoLocalization = false;
// return Response.CreateFromCommand(command, taskPlanner.AutoLocalization);
//}
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, false));
}
private void Form1_Load(object sender, EventArgs e)
{
string buidlDate = " - LastBuild: " + File.GetLastWriteTime(System.Reflection.Assembly.GetExecutingAssembly().Location).ToString("G");
this.Text += buidlDate;
string logFileName = "ST-PLN " + DateTime.Now.ToString("yyyy-MM-dd hh.mm.ss") + ".log";
TextBoxStreamWriter.DefaultLog = new TextBoxStreamWriter(txtConsole, logFileName, 1000);
TextBoxStreamWriter.DefaultLog.DefaultPriority = 0;
// To use with newer version of Robotics.dll
TextBoxStreamWriter.DefaultLog.VerbosityThreshold = 6;
//TextBoxStreamWriter.DefaultLog.TextBoxVerbosityThreshold = 6;
TBWriter.Spaced(" All mighty Simple Task Planner");
TextBoxStreamWriter.DefaultLog.WriteLine(buidlDate);
// To use with newer version of Robotics.dll.
nupConsoleThreshold.Value = TextBoxStreamWriter.DefaultLog.VerbosityThreshold;
//nupConsoleThreshold.Value = TextBoxStreamWriter.DefaultLog.TextBoxVerbosityThreshold;
}
protected override Response AsyncTask(Command command)
{
bool success;
string objToPoint;
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
objToPoint = command.Parameters;
success = taskPlanner.Cmd_PointAtObject(objToPoint);
if (success)
{
TBWriter.Info1(command.CommandName + " succesfully executed");
}
else
{
TBWriter.Info1(command.CommandName + " execution failed");
}
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
return(Response.CreateFromCommand(command, success));
}
protected override Response AsyncTask(Command command)
{
TBWriter.Spaced(" >>> Received " + command.CommandName + " , received = [" + command.Parameters + "]");
string[] splitParams = command.Parameters.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
if (splitParams.Length != 4)
{
TBWriter.Error("Not enough parameters found.");
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
this.cmdMan.Busy = false;
return(Response.CreateFromCommand(command, false));
}
string armToTake = splitParams[0];
double x;
double y;
double z;
if (!double.TryParse(splitParams[1], out x))
{
TBWriter.Error("Cant parse X= " + splitParams[1]);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
this.cmdMan.Busy = false;
return(Response.CreateFromCommand(command, false));
}
if (!double.TryParse(splitParams[2], out y))
{
TBWriter.Error("Cant parse Y= " + splitParams[2]);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
this.cmdMan.Busy = false;
return(Response.CreateFromCommand(command, false));
}
if (!double.TryParse(splitParams[3], out z))
{
TBWriter.Error("Cant parse Z= " + splitParams[3]);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
this.cmdMan.Busy = false;
return(Response.CreateFromCommand(command, false));
}
bool success;
if (armToTake.Contains("left"))
{
success = this.taskplanner.takeXYZ_LEFT(new WorldObject("unknown", new Vector3(x, y, z)));
}
else if (armToTake.Contains("right"))
{
success = this.taskplanner.takeXYZ_RIGHT(new WorldObject("unknown", new Vector3(x, y, z)));
}
else
{
TBWriter.Error("No arm to take defined = " + armToTake);
TBWriter.Spaced(" Terminated " + command.CommandName + " , sending = [" + command.Parameters + "] <<<");
this.cmdMan.Busy = false;
return(Response.CreateFromCommand(command, false));
}
TBWriter.Spaced(" Terminated " + command.CommandName + ", success{" + success + "} , sending = [" + command.Parameters + "] <<<");
this.cmdMan.Busy = false;
return(Response.CreateFromCommand(command, success));
}
void sharedVarSubscriptionThreadTask()
{
bool printConnectedModules = false;
while (!successSharedVarSuscription)
{
if (!sharedVarLoadedFromBlackBoard)
{
TBWriter.Write(9, "Trying to Load SharedVars form Blackboard ");
string message;
int varsLoaded;
varsLoaded = cmdMan.SharedVariables.LoadFromBlackboard(1000, out message);
if (varsLoaded > 0)
{
sharedVarLoadedFromBlackBoard = true;
TBWriter.Info1(" Loaded " + varsLoaded.ToString() + " shared Variables : " + message);
}
else
{
sharedVarLoadedFromBlackBoard = false;
TBWriter.Write(7, "ERROR : No shared Variables loaded " + message);
}
continue;
}
if (!successSharedVarConnected)
{
successSharedVarConnected = suscribeSharedVarConnected();
}
if (!successSharVarHdPos)
{
successSharVarHdPos = suscribeSharVarHdPos();
}
if (!successSharVarOdometryPos)
{
successSharVarOdometryPos = suscribeSharVarOdometryPos();
}
if (!successSharVarSkeletons)
{
successSharVarSkeletons = suscribeSharVarSkeletons();
}
if (!successSharVarTorso)
{
successSharVarTorso = suscribeSharVarTorso();
}
if (!successSharVarRobotRegion)
{
successSharVarRobotRegion = suscribeSharVarRobotRegion();
}
successSharedVarSuscription = successSharedVarConnected & successSharVarHdPos & successSharVarOdometryPos & successSharVarSkeletons & successSharVarTorso & successSharVarRobotRegion;
Thread.Sleep(300);
if (successSharedVarConnected && !printConnectedModules)
{
bool success;
string connectedModules;
do
{
success = sharedVarConnected.TryRead(out connectedModules, 500);
} while (!success);
TBWriter.Write(1, String.Empty);
TBWriter.Write(1, "\tModules Connected :");
taskPlanner.robot.ActualizeConnectedModules(connectedModules);
printConnectedModules = true;
}
}
TBWriter.Spaced("Simple Task Planner now Ready");
cmdMan.Ready = true;
TBWriter.Write(1, "Waiting for Command . . .");
TBWriter.Write(1, "");
TBWriter.Write(1, @"\.>");
}
