public bool ActualizeConnectedModules(string SharedVar)
{
if (SharedVar == null)
{
TBWriter.Write("ERROR : Can't actualize CONNECTED modules, null string");
return(false);
}
foreach (string modName in modules.Keys)
{
if (SharedVar.Contains(modName))
{
if (!modules[modName].IsConnected)
{
modules[modName].IsConnected = true;
TBWriter.Write(5, " Module " + modName + " is now connected ");
}
}
else
if (modules[modName].IsConnected)
{
modules[modName].IsConnected = false;
TBWriter.Write(3, " > Warning : module " + modName + " is disconnected");
}
}
TBWriter.SharedVarAct("Actualized connected Shared Var");
return(true);
}
public bool ActualizePositionStatus(string armPosition)
{
Vector3 armPos;
Vector3 armOri;
double elbow;
bool success;
success = Parse.ArmPosition(armPosition, out armPos, out armOri, out elbow);
if (success)
{
this.position = new Vector3(armPos);
this.orientation = new Vector3(armOri);
this.elbow = elbow;
TBWriter.Write(7, "Successfully actualized ARM position , orientation and elbow");
return(true);
}
else
{
TBWriter.Write("ERROR : Can't actualize ARM position, orientation and elbow");
return(false);
}
}
private void sharedVarTorso_ValueChanged(SharedVariableSubscriptionReport <double[]> report)
{
if (taskPlanner.robot.torso.ActualizeTorsoStatus(report.Value))
{
TBWriter.Write(8, "Actualized Shared Var successfully torso; " + report.Variable.Name);
}
else
{
TBWriter.Write("ERROR ! Cant act5ualize Shared Var torso; " + report.Variable.Name);
}
}
private void sharedVarHdPos_ValueChanged(SharedVariableSubscriptionReport <double[]> report)
{
if (taskPlanner.robot.head.ActualizeHeadPosition(report.Value))
{
TBWriter.Write(8, "Actualized Shared Var successfully: head; " + report.Variable.Name);
}
else
{
TBWriter.Write("ERROR ! Cant actualize Shared Var: head; " + report.Variable.Name);
}
}
private void sharedVarOdometryPos_ValueChanged(SharedVariableSubscriptionReport <double[]> report)
{
if (taskPlanner.robot.hardwareMan.MobileBase.ActualizeOdometryPosition(report.Value))
{
TBWriter.Write(8, "Actualized Shared Var successfully: odometry; " + report.Variable.Name);
}
else
{
TBWriter.Write("ERROR ! Cant actualize Shared Var: odometry; " + report.Variable.Name);
}
}
public void ActualizeSkeletonsSharedVar(string sharedVar)
{
if (sharedVar == null)
{
TBWriter.Write("ERROR !!! Can't actualize SKELETONS SharedVar : String is null");
return;
}
this.skeletonsString = sharedVar;
TBWriter.Write(9, " Successfully actualized SKELETONS sharedVar");
return;
}
public bool ActualizeTorsoStatus(double[] sharedVar)
{
bool success = Parse.TorsoPosition(sharedVar, out this.elevation, out this.pan);
if (success)
{
TBWriter.Write(7, " Successfully Actualized Torso Position");
}
else
{
TBWriter.Write("ERROR : Can't actulize TORSO position");
}
return(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 OdometryPosition(double[] sharedVar, out Vector3 position, out double orientation)
{
if ((sharedVar == null) || (sharedVar.Length != 3))
{
TBWriter.Warning1("Can't parse odometryPos, null value or invalid lenght");
position = null;
orientation = double.NaN;
return(false);
}
position = new Vector3(sharedVar[0], sharedVar[1], 0);
orientation = sharedVar[2];
TBWriter.Write(9, "Successfully parsed odometryPos [ x= " + position.X.ToString("0.00") + " , y= " + position.Y.ToString("0.00") + " , orientation= " + orientation.ToString("0.00") + " ]");
return(true);
}
public static bool findArmInfo(string information, out Vector3 position)
{
position = null;
string[] armInfo;
char[] separator = { ' ' };
armInfo = information.Split(separator, StringSplitOptions.RemoveEmptyEntries);
if (armInfo.Length == 0)
{
TBWriter.Warning1("findArmInfo can't parse, lenght is 0");
return(false);
}
double tempX;
double tempY;
double tempZ;
if (!double.TryParse(armInfo[1], out tempX))
{
TBWriter.Warning1("findArmInfo can't parse position X");
position = null;
return(false);
}
if (!double.TryParse(armInfo[2], out tempY))
{
TBWriter.Warning1("findArmInfo can;t parse position Y");
position = null;
return(false);
}
if (!double.TryParse(armInfo[3], out tempZ))
{
TBWriter.Warning1("findArmInfo can't parse position Z");
position = null;
return(false);
}
position = new Vector3(tempX, tempY, tempZ);
TBWriter.Write(9, "Succesfully parsed findArmInfo [ " + position.ToString() + " ]");
return(true);
}
public static bool TorsoPosition(double[] sharedVar, out double elevation, out double pan)
{
if ((sharedVar == null) || (sharedVar.Length != 2))
{
TBWriter.Warning1("Can't parse torsoPos, null value or invalid lenght");
elevation = 0.7;
pan = 0.0;
return(false);
}
elevation = sharedVar[0];
pan = sharedVar[1];
TBWriter.Write(9, "Successfully parsed torsoPos [ elevation=" + elevation.ToString("0.00") + " , pan=" + pan.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);
}
public static bool OdometryPosition(string parameters, out Vector3 position, out double orientation)
{
position = null;
orientation = double.NaN;
string[] odometry;
char[] separator = { ' ' };
odometry = parameters.Split(separator, StringSplitOptions.RemoveEmptyEntries);
if (odometry.Length == 0)
{
TBWriter.Warning1("Cant parse odometryPos, info lenght is 0");
return(false);
}
double tempX;
double tempY;
double tempOrientation;
if (!double.TryParse(odometry[0], out tempX))
{
TBWriter.Warning1("Cant Parse Position X");
position = null;
return(false);
}
if (!double.TryParse(odometry[1], out tempY))
{
TBWriter.Warning1("Cant Parse Position T");
return(false);
}
if (!double.TryParse(odometry[2], out tempOrientation))
{
TBWriter.Warning1("Cant Parse Orientation");
position = null;
return(false);
}
position = new Vector3(tempX, tempY, 0);
orientation = tempOrientation;
TBWriter.Write(9, "Successfully parsed odometryPos [ x= " + position.X.ToString("0.00") + " , y= " + position.Y.ToString("0.00") + " , orientation= " + orientation.ToString("0.00") + " ]");
return(true);
}
public bool ActualizeOdometryPosition(double[] positionAndOrientation)
{
if ((positionAndOrientation == null) || (positionAndOrientation.Length != 3))
{
TBWriter.Warning1("Can't Actualize OdometryPos, value is null or invalid lenght");
return(false);
}
else
{
this.position = new Vector3();
this.position.X = positionAndOrientation[0];
this.position.Y = positionAndOrientation[1];
this.orientation = positionAndOrientation[2];
TBWriter.Write(9, "Actualized OdometryPos [ position=" + position.ToString() + " , orientation=" + orientation.ToString("0.00") + " ]");
return(true);
}
}
public static bool NodeInfo(string nodeInfo, out Vector3 errorPosition, out double errorOrientation)
{
errorPosition = null;
errorOrientation = double.NaN;
string[] nodeError;
char[] separator = { ' ' };
nodeError = nodeInfo.Split(separator, StringSplitOptions.RemoveEmptyEntries);
if (nodeError.Length == 0)
{
TBWriter.Warning1("Cant parse nodeInfo, lenght is 0");
return(false);
}
double tempX;
double tempY;
double tempOrientation;
if (!double.TryParse(nodeError[0], out tempX))
{
TBWriter.Warning1("Can't parse errorX");
return(false);
}
if (!double.TryParse(nodeError[1], out tempY))
{
TBWriter.Warning1("Cant parse errorY");
return(false);
}
if (!double.TryParse(nodeError[2], out tempOrientation))
{
TBWriter.Warning1("Can't parse errorOrientation");
return(false);
}
errorPosition = new Vector3(tempX, tempY, 0);
errorOrientation = tempOrientation;
TBWriter.Write(9, "Successfully parsed NodeInfo [ x=" + tempX.ToString() + ", y=" + tempY.ToString() + ", orientation=" + tempOrientation.ToString() + " ]");
return(true);
}
private Matrix CalculateInverseHomogeneous(Matrix homogeneousMat)
{
if ((homogeneousMat.Rows != 4) || (homogeneousMat.Columns != 4))
{
TBWriter.Write("Error : Cant get Inverse Homogeneous Matrix, invalid order");
return(null);
}
Matrix rotM = getRotMatrixFromHomogen(homogeneousMat);
rotM = rotM.Transpose;
Matrix transM = getTransMatrixFromHomogen(homogeneousMat);
transM = (-1) * rotM * transM;
Matrix InversM = setHomogeneousMatrix(rotM, transM);
return(InversM);
}
public STPlanner()
{
TBWriter.Spaced("Simple Task Planner, Version 0.98798");
TextBoxStreamWriter.DefaultLog.WriteLine(0, "asdasd as asd ");
TextBoxStreamWriter.DefaultLog.WriteLine(0, "Builded 16/feb/13 , 15:351 p.m.");
TextBoxStreamWriter.DefaultLog.WriteLine(0, "");
cmdMan = new STPCommandManager();
cnnMan = new ConnectionManager(connectionPort, this.cmdMan);
TBWriter.Write(" Connection Port = " + connectionPort.ToString());
this.cnnMan.ClientConnected += new System.Net.Sockets.TcpClientConnectedEventHandler(cnnMan_ClientConnected);
TBWriter.Spaced(TBWriter.time() + " Waiting for Blackboard Connection . . .");
taskPlanner = new TaskPlanner(cmdMan, this);
LoadCommands();
sharedVarConnected = new StringSharedVariable("connected");
sharedVarSkeletons = new StringSharedVariable("skeletons");
sharedVarOdometryPos = new DoubleArraySharedVariable("odometryPos");
sharedVarHdPos = new DoubleArraySharedVariable("hd_pos");
sharedVarTorso = new DoubleArraySharedVariable("torsoPosition");
sharedVarRobotRegion = new StringSharedVariable("robotRoom");
//cmdMan.SharedVariablesLoaded += new SharedVariablesLoadedEventHandler(cmdMan_SharedVariablesLoaded);
cnnMan.Start();
TBWriter.Write(3, " > Connection Manager Started");
cmdMan.Start();
TBWriter.Write(3, " > Command Manager Started");
//if(!sharedVarLoadEvent) TBWriter.Spaced(" Waiting for Shared Variables Load from BlackBoard . . .")
}
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;
}
}
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, @"\.>");
}
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);
}
