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 LoadSceneMap(string filePath) { TBWriter.Info4("Loading SceneMap file [" + filePath + "] ..."); if (!File.Exists(filePath)) { TBWriter.Error("SceneMap file [" + filePath + "] doesn't exist"); return(false); } FileStream fs = File.Open(filePath, FileMode.Open, FileAccess.Read); XmlSerializer serializer = new XmlSerializer(typeof(SceneNode[])); SceneNode[] tempNodeArray = (SceneNode[])serializer.Deserialize(fs); fs.Close(); this.nodesList.Clear(); foreach (SceneNode node in tempNodeArray) { this.nodesList.Add(node.Name, node); } this.sceneMapfile = filePath; TBWriter.Info4("SceneMap loaded successfully"); return(true); }
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); } }
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)); }
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) { 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)); }
public bool ActualizeHeadPosition(double[] panAndTiltAngles) { if ((panAndTiltAngles == null) || (panAndTiltAngles.Length != 2)) { TBWriter.Error("Can't ActualizeHeadPosition, null value or invalid lenght"); return(false); } else { pan = panAndTiltAngles[0]; tilt = panAndTiltAngles[1]; TBWriter.Write(9, "Actualized headPos [ pan=" + pan.ToString("0.00") + " , tilt=" + tilt.ToString("0.00") + " ]"); return(true); } }
public static bool HeadPosition(double[] sharedVar, out double pan, out double tilt) { if ((sharedVar == null) || (sharedVar.Length != 2)) { TBWriter.Error("Can't parse headPos, null value or invalid lenght"); pan = double.NaN; tilt = double.NaN; return(false); } pan = sharedVar[0]; tilt = sharedVar[1]; TBWriter.Write(9, "Successfully parsed headPos [ pan=" + pan.ToString("0.00") + " , tilt=" + tilt.ToString("0.00") + " ]"); return(true); }
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)); }
private void cmdMan_SharedVariablesLoaded(CommandManager cmdMan) { if (successSharedVarSuscription) { return; } TBWriter.Write(3, " CmdMan : Shared Variable Loaded succesfully, trying to suscribe . . ."); successSharedVarConnected = suscribeSharedVarConnected(); successSharVarHdPos = suscribeSharVarHdPos(); successSharVarOdometryPos = suscribeSharVarOdometryPos(); successSharVarSkeletons = suscribeSharVarSkeletons(); successSharVarTorso = suscribeSharVarTorso(); successSharVarRobotRegion = suscribeSharVarRobotRegion(); taskPlanner.robot.ActualizeConnectedModules(sharedVarConnected.Value); successSharedVarSuscription = successSharedVarConnected & successSharVarHdPos & successSharVarOdometryPos & successSharVarSkeletons & successSharVarTorso & successSharVarRobotRegion; if (successSharedVarSuscription) { cmdMan.Ready = true; TBWriter.Info2(" All Shared Variable Suscribed Succesfully"); TBWriter.Write(""); TBWriter.Write("----- Systems Ready ----- "); TBWriter.Write(""); } else { cmdMan.Ready = false; TBWriter.Error(": Shared Variable Suscription Failed"); TBWriter.Warning1("System is not fully ready"); cmdMan.Ready = false; } }
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)); } }
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)); }
public static bool FindObjectOnTableInfo(string objectsString, out WorldObject[] objects) { string name; double tempX; double tempY; double tempZ; double distanceFromTable; int numberOfObjectsFounded; string[] objInfo; char[] separator = { ' ' }; objInfo = objectsString.Split(separator, StringSplitOptions.RemoveEmptyEntries); if (objInfo.Length < 1) { TBWriter.Warning1("FindObjectsOnTableInfo can't parse, string lenght is 0 "); objects = null; return(false); } if (objInfo.Length % 5 != 0) { TBWriter.Warning1("FindObjectsOnTableInfo can't parse, objects.lenght%5 != 0"); objects = null; return(false); } numberOfObjectsFounded = objInfo.Length / 5; objects = new WorldObject[numberOfObjectsFounded]; int i = 0; for (int j = 0; j < objects.Length; j++) { name = objInfo[i]; i++; if (!double.TryParse(objInfo[i], out tempX)) { TBWriter.Error("FindObjectsOnTableInfo, can't parse position X"); objects = null; return(false); } i++; if (!double.TryParse(objInfo[i], out tempY)) { TBWriter.Error("FindObjectsOnTableInfo, can't parse position Y"); objects = null; return(false); } i++; if (!double.TryParse(objInfo[i], out tempZ)) { TBWriter.Error("FindObjectsOnTableInfo, can't parse position Z"); objects = null; return(false); } i++; if (!double.TryParse(objInfo[i], out distanceFromTable)) { TBWriter.Error("FindObjectsOnTableInfo, can't parse distance from Table"); objects = null; return(false); } i++; WorldObject newObject = new WorldObject(name, new Vector3(tempX, tempY, tempZ), distanceFromTable); TBWriter.Write(9, "Successfully parsed FindObjectsOnTableInfo [ " + newObject.ToString() + " ]"); objects[j] = newObject; } return(true); }
public static bool Skeletons(string sharedSkeletons, out Skeleton[] skeletons) { /* The serialized data hast the following form (only hex-characters (0-F), spaces * and dots are only to ilustrate the format): * 0x 0 ... 0 0 ... 0 0 ... 0 0 ... 0 0 ... 0 . . . 0 ... 0 0 ... 0 0 ... 0 0 ... 0 * |__________| |_________| |_________| |_________| |_________| |_________| |_________| |_________| |_________| * int32 count int32 ID1 short x short y1 short z1 int32 IDn short xn short yn short zn * 4 bytes 4 bytes 8 bytes 8 bytes 8 bytes 4 bytes 8 bytes 8 bytes * 8 chars 8 chars 16 chars 16 chars 16 chars 8 chars 16 chars 16 chars 16 chars * The length of the string is given by length = 8 + (8+20*12)*count * where count is the number of found persons*/ if ((sharedSkeletons == null) || (sharedSkeletons == "") || (sharedSkeletons.Length < 256) || ((sharedSkeletons.Length % 2) != 0)) { TBWriter.Error("Can't Parse Skeleton string, string null or invalid length"); skeletons = null; return(false); } int count; int id; double x; double y; double z; byte[] bytes; int ix = 0; ix = sharedSkeletons.StartsWith("0x", StringComparison.InvariantCultureIgnoreCase) ? 2 : 0; bytes = new byte[(sharedSkeletons.Length - ix) / 2]; for (int i = 0; i < bytes.Length; ++i, ix += 2) { bytes[i] = Byte.Parse(sharedSkeletons.Substring(ix, 2), System.Globalization.NumberStyles.HexNumber, System.Globalization.CultureInfo.InvariantCulture); } ix = 0; count = BitConverter.ToInt32(bytes, ix); ix += 4; if ((count < 0) || (((sharedSkeletons.Length - 8) / 248) != count)) { TBWriter.Error("Can't Parse Skeleton string, skeleton lenght error"); skeletons = null; return(false); } skeletons = new Skeleton[count]; for (int i = 0; i < count; i++) { //The first four bytes are the ID id = BitConverter.ToInt32(bytes, ix); ix += 4; x = BitConverter.ToInt16(bytes, ix); ix += 2; y = BitConverter.ToInt16(bytes, ix); ix += 2; z = BitConverter.ToInt16(bytes, ix); ix += 2 + 6 * 19; skeletons[i] = new Skeleton(id, new Vector3(x / 1000, y / 1000, z / 1000)); TBWriter.Write(9, "Successfully parsed skeleton [ " + skeletons[i].ToString() + " ]"); } return(true); }