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, @"\.>"); }