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 ArmPosition(string armPosition, out Vector3 position, out Vector3 orientation, out double elbow) { string[] armValues; char[] separator = { ' ' }; if (armPosition == null) { TBWriter.Warning1("Can't parse armPosition, null value"); position = null; orientation = null; elbow = 0; return(false); } armValues = armPosition.Split(separator, StringSplitOptions.RemoveEmptyEntries); if (armValues.Length != 7) { TBWriter.Warning1("Can't parse armPosition, invalid lenght"); position = null; orientation = null; elbow = 0; return(false); } double tempX; double tempY; double tempZ; double tempRoll; double tempPitch; double tempYaw; double tempElbow; if (!double.TryParse(armValues[0], out tempX)) { TBWriter.Warning1("Can't parse armPosition X"); position = null; orientation = null; elbow = 0; return(false); } if (!double.TryParse(armValues[1], out tempY)) { TBWriter.Warning1("Can't parse armPosition Y"); position = null; orientation = null; elbow = 0; return(false); } if (!double.TryParse(armValues[2], out tempZ)) { TBWriter.Warning1("Can't parse armPosition Z"); position = null; orientation = null; elbow = 0; return(false); } if (!double.TryParse(armValues[3], out tempRoll)) { TBWriter.Warning1("Can't parse armPosition Roll"); position = null; orientation = null; elbow = 0; return(false); } if (!double.TryParse(armValues[4], out tempPitch)) { TBWriter.Warning1("Can't parse armPosition Pitch"); position = null; orientation = null; elbow = 0; return(false); } if (!double.TryParse(armValues[5], out tempYaw)) { TBWriter.Warning1("Can't parse armPosition Yaw"); position = null; orientation = null; elbow = 0; return(false); } if (!double.TryParse(armValues[6], out tempElbow)) { TBWriter.Warning1("Can't parse armPosition Elbow"); position = null; orientation = null; elbow = 0; return(false); } position = new Vector3(tempX, tempY, tempZ); orientation = new Vector3(tempRoll, tempPitch, tempYaw); elbow = tempElbow; TBWriter.Write(9, "Succesfully parsed armPosition [ " + tempX.ToString() + " " + tempY.ToString() + " " + tempZ.ToString() + " " + tempRoll.ToString() + " " + tempPitch.ToString() + " " + tempYaw.ToString() + " " + tempElbow.ToString() + " ]"); return(true); }
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); }
/// <summary> /// Try to Parse human info from minoru, /// </summary> /// <param name="humanInfo">string with information</param> /// <param name="humanName"></param> /// <param name="facePosition"></param> /// <returns></returns> public static bool humanInfo(string humanInfo, out string humanName, out Vector3 facePosition, out double humanPan, out double humanTilt) { humanPan = double.MaxValue; humanTilt = double.MaxValue; double distanceToPerson = 1; string[] parts = humanInfo.Split(null as char[], StringSplitOptions.RemoveEmptyEntries); if (parts.Length == 1) { humanName = parts[0]; facePosition = new Vector3(0, 0, distanceToPerson); TBWriter.Write(9, "Successfully parsed humanInfo [ Name:" + humanName + " , FacePos:" + facePosition.ToString() + " ]"); humanPan = 0; humanTilt = 0; return(true); } if (parts.Length == 3) { double pan; double tilt; if (!Double.TryParse(parts[1], out pan)) { TBWriter.Warning1("humanInfo can't parse pan"); humanName = ""; facePosition = null; return(false); } if (!Double.TryParse(parts[2], out tilt)) { TBWriter.Warning1("humanInfo can't parse tilt"); humanName = ""; facePosition = null; return(false); } humanPan = pan; humanTilt = tilt; humanName = parts[0]; facePosition = new Vector3(Vector3.SphericalToCartesian(new Vector3(distanceToPerson, pan, tilt))); TBWriter.Write(9, "Successfully parsed humanInfo [ Name:" + humanName + " , FacePos:" + facePosition.ToString() + " ]"); return(true); } if (parts.Length == 4) { double pan; double tilt; if (!Double.TryParse(parts[2], out pan)) { TBWriter.Warning1("humanInfo can't parse pan"); humanName = ""; facePosition = null; return(false); } if (!Double.TryParse(parts[3], out tilt)) { TBWriter.Warning1("humanInfo can't parse tilt"); humanName = ""; facePosition = null; return(false); } humanPan = pan; humanTilt = tilt; humanName = parts[1]; facePosition = new Vector3(Vector3.SphericalToCartesian(new Vector3(distanceToPerson, pan, tilt))); TBWriter.Write(9, "Successfully parsed humanInfo [ Name:" + humanName + " , FacePos:" + facePosition.ToString() + " ]"); return(true); } TBWriter.Warning1("humanInfo can't parse, parts.lenght < 0 "); humanName = ""; facePosition = null; return(false); }
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); }
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, @"\.>"); }