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 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)); }
public bool LoadSceneNodeList(string filePath) { TBWriter.Info8("Loading SceneNodeList ..."); SceneNode[] sceneNodeAray; XmlSerializer serializer; if (!File.Exists(filePath)) { TBWriter.Error("Can't SceneNodes file : " + filePath); return(false); } FileStream fs = File.Open(filePath, FileMode.Open, FileAccess.Read); serializer = new XmlSerializer(typeof(SceneNode[])); sceneNodeAray = (SceneNode[])serializer.Deserialize(fs); fs.Close(); sceneNodes.Clear(); sceneNodes.AddRange(sceneNodeAray); TBWriter.Info1("Successfully loaded ScenenNodeList " + filePath + " . NumberOfNodes = " + sceneNodeAray.Length); return(true); }
private bool suscribeSharVarTorso() { TBWriter.Info8("Trying to suscribe to Shared Variable: " + sharedVarTorso.Name); try { if (cmdMan.SharedVariables.Contains(sharedVarTorso.Name)) { this.sharedVarTorso = (DoubleArraySharedVariable)(cmdMan.SharedVariables[sharedVarTorso.Name]); TBWriter.Info9("Shared Variable already exist, refering to it: " + sharedVarTorso.Name); } else { cmdMan.SharedVariables.Add(this.sharedVarTorso); TBWriter.Info9("Shared Variable doesnt exist, adding it: " + sharedVarTorso.Name); } this.sharedVarTorso.Subscribe(SharedVariableReportType.SendContent, SharedVariableSubscriptionType.WriteOthers); sharedVarTorso.ValueChanged += new SharedVariableSubscriptionReportEventHadler <double[]>(sharedVarTorso_ValueChanged); TBWriter.Info1("Suscribed to Shared Variable: " + sharedVarTorso.Name); return(true); } catch (Exception ex) { TBWriter.Error("Can't suscribe to Shared Variable: " + sharedVarTorso.Name + " ; Msg= " + ex.Message); return(false); } }
private bool suscribeSharVarSkeletons() { TBWriter.Info8("Trying to suscribe to Shared Variable: " + sharedVarSkeletons.Name); try { if (cmdMan.SharedVariables.Contains(this.sharedVarSkeletons.Name)) { this.sharedVarSkeletons = (StringSharedVariable)(this.cmdMan.SharedVariables[this.sharedVarSkeletons.Name]); TBWriter.Info9("Shared Variable already exist, refering to it: " + sharedVarSkeletons.Name); } else { cmdMan.SharedVariables.Add(this.sharedVarSkeletons); TBWriter.Info9("Shared Variable doesnt exist, adding it: " + sharedVarSkeletons.Name); } sharedVarSkeletons.Subscribe(SharedVariableReportType.SendContent, SharedVariableSubscriptionType.WriteOthers); sharedVarSkeletons.ValueChanged += new SharedVariableSubscriptionReportEventHadler <string>(sharedVarSkeletons_ValueChanged); TBWriter.Info1("Suscribed to Shared Variable: " + sharedVarSkeletons.Name); return(true); } catch (Exception e) { TBWriter.Error("Can't suscribe to Shared Variable: " + sharedVarSkeletons.Name + " ; Msg= " + e.Message); return(false); } }
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)); }
private void btnTest_Click(object sender, EventArgs e) { Vector3[] array = new Vector3[100000]; Vector3[] arrayCopy; Vector3[] arrayclone; Vector3[] arrayToArray; Vector3[] arrayFor; Random ran = new Random(); DateTime initialTime; for (int i = 0; i < array.Length; i++) { array[i] = new Vector3(ran.NextDouble(), ran.NextDouble(), ran.NextDouble()); } initialTime = DateTime.Now; array.CopyTo(arrayCopy = new Vector3[array.Length], 0); TBWriter.Info1("array.CopyTo TIME : " + DateTime.Now.Subtract(initialTime).TotalMilliseconds); initialTime = DateTime.Now; arrayclone = (Vector3[])array.Clone(); TBWriter.Info1("(int[])array.Clone() TIME : " + DateTime.Now.Subtract(initialTime).TotalMilliseconds); initialTime = DateTime.Now; arrayToArray = array.ToArray(); TBWriter.Info1("array.ToArray() TIME : " + DateTime.Now.Subtract(initialTime).TotalMilliseconds); initialTime = DateTime.Now; arrayFor = new Vector3[array.Length]; for (int i = 0; i < array.Length; i++) { arrayFor[i] = new Vector3(array[i]); } TBWriter.Info1("arrayFor TIME : " + DateTime.Now.Subtract(initialTime).TotalMilliseconds); TBWriter.Info1("array[1] = " + array[1].ToString()); TBWriter.Info1("arrayCopy[1] = " + arrayCopy[1].ToString()); arrayCopy[1].X = (ran.NextDouble()); arrayCopy[1].Y = (ran.NextDouble()); arrayCopy[1].Z = (ran.NextDouble()); TBWriter.Info1("arrayCopy[1] = " + arrayCopy[1].ToString()); TBWriter.Info1("array[1] = " + array[1].ToString()); TBWriter.Info1("arrayclone[1] = " + arrayclone[1].ToString()); TBWriter.Info1("arrayToArray[1] = " + arrayToArray[1].ToString()); TBWriter.Info1("arrayFor[1] = " + arrayFor[1].ToString()); //SceneMaTBWriter.Info1("arrayToArray[1] = " + arrayToArray[1].ToString());nager map = new SceneManager //(); //map.AddNode( new SceneNode( "myNode" , Vector3.UnitX)); //map.Nodes[0].AddFrame( 0, 0, 0.8); //map.Nodes[0].AddFrame(1, 11, 0.8); //map.Nodes.Add( new SceneNode( "myAnotherNode" , new Vector3(2, 2, 2))); //map.Nodes[1].AddFrame(2, 3, 1.8); //map.Nodes[1].AddFrame(2,1, 0); //map.Nodes[1].AddFrame(0, 0, 0); //map.SaveSceneMap(); }
protected override Response AsyncTask(Command command) { string parameters = command.Parameters; bool success; TBWriter.Info1("New Command {detectcubes} , parameters{" + parameters + "}. Parsing parameters..."); if (parameters.Contains("all")) { TBWriter.Info1("Received detectcubes(all)..."); List <WorldObject> cubesInVisionCoor; List <WorldObject> cubesInRobotCoor = new List <WorldObject>(); if (!this.cmdMan.HEAD_lookat(0.00, -1.1, 5000)) { this.cmdMan.HEAD_lookat(0.00, 0.00, 5000); if (!this.cmdMan.HEAD_lookat(0.00, -1.1, 5000)) { TBWriter.Error("CANT MOVE HEAD... RETURNING FALSE"); return(Response.CreateFromCommand(command, false)); } } if (!this.cmdMan.OBJ_FND_findcubes(out cubesInVisionCoor)) { success = false; } else { success = true; this.taskPln.ActualizeHeadPos(); foreach (WorldObject o in cubesInVisionCoor) { Vector3 robCoor = this.taskPln.robot.TransHeadKinect2Robot(o.Position); cubesInRobotCoor.Add(new WorldObject(o.Name, robCoor)); } } string resp = ""; foreach (WorldObject o in cubesInRobotCoor) { resp += o.Name + " " + o.Position.X + " " + o.Position.Y + " " + o.Position.Z + " "; } TBWriter.Info1("Sending detectcubes(all) => Success={" + success + "} , params={" + resp + "}"); command.Parameters = resp; return(Response.CreateFromCommand(command, success)); } else if (parameters.Contains("cubestable")) { string direction; if (parameters.Contains("left")) { direction = "left"; } else { direction = "right"; } TBWriter.Info1("Received detectcubes(cubestable , " + direction + ") ..."); WorldObject emptyPoint; Vector3 emptyInRobotCoor; if (!this.cmdMan.OBj_FND_findcubesEmptyPoint(direction, out emptyPoint)) { success = false; } else { this.taskPln.ActualizeHeadPos(); emptyInRobotCoor = this.taskPln.robot.TransHeadKinect2Robot(emptyPoint.Position); command.Parameters = "cubestable " + emptyInRobotCoor.X + " " + emptyInRobotCoor.Y + " " + emptyInRobotCoor.Z; success = true; } TBWriter.Info1("Sending detectcubes(cubestable , " + direction + ") => Success={" + success + "} , params={" + command.Parameters + "}"); return(Response.CreateFromCommand(command, success)); } else { TBWriter.Info1("Received detectcubes(color=" + parameters + ")..."); WorldObject cube; if (!this.cmdMan.OBJ_FND_findcubes(parameters, out cube)) { success = false; } else { this.taskPln.ActualizeHeadPos(); Vector3 cubeInRobotCoor = this.taskPln.robot.TransHeadKinect2Robot(cube.Position); command.Parameters = cube.Name + " " + cubeInRobotCoor.X + " " + cubeInRobotCoor.Y + " " + cubeInRobotCoor.Z; success = true; } TBWriter.Info1("Sending detectcubes(color) => Success={" + success + "} , params={" + command.Parameters + "}"); 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, @"\.>"); }