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