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);
}
