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