private void AuthenticationButton_OnClicked(object sender, EventArgs e)
{
MainFrame.IsVisible = false;
RobotImage.ScaleTo(3, 200);
RobotImage.TranslateTo(0, 100, 200);
}
public TargetListMessage(int robotID, RobotImage image, double timeStamp, int targetID, TargetTypes type)
{
this.robotID = robotID;
this.image = image;
this.timeStamp = timeStamp;
this.targetID = targetID;
this.type = type;
}
public TargetListMessage(int robotID, RobotImage image, double timeStamp, int targetID, Vector2 targetPose, Matrix targetCov, TargetTypes type)
{
this.robotID = robotID;
this.image = image;
this.timeStamp = timeStamp;
this.targetID = targetID;
this.targetPose = targetPose;
this.type = type;
this.cov = targetCov;
}
public TargetListWithImageMessage(int robotID, RobotImage img, List <RobotPose> targetState, List <RobotPose> lastPose, List <Matrix> targetCov,
List <Vector2> pixelLTCorner, List <Vector2> pixelRBCorner, List <int> targetIDs, double timeStamp)
{
this.robotID = robotID;
this.pixelLTCorner = pixelLTCorner;
this.pixelRBCorner = pixelRBCorner;
this.timeStamp = timeStamp;
this.targetID = targetIDs;
this.img = img;
this.state = targetState;
this.cov = targetCov;
this.lastPose = lastPose;
}
private void ParseImagePacket(BinaryReader br)
{
UDPCameraMsg msg = new UDPCameraMsg();
msg.index = br.ReadInt32();
msg.timestamp = br.ReadDouble();
msg.width = br.ReadInt32();
msg.height = br.ReadInt32();
msg.size = br.ReadInt32();
msg.id = br.ReadInt32();
byte[] jpgimg = br.ReadBytes(msg.size);
MemoryStream ms = new MemoryStream(jpgimg);
Bitmap bmp = (Bitmap)System.Drawing.Bitmap.FromStream(ms);
RobotImage ri = new RobotImage(msg.id, bmp, msg.timestamp);
if (ImageReceived != null)
{
ImageReceived(this, new TimestampedEventArgs <RobotImage>(msg.timestamp, ri));
}
}
void queryClient_MsgReceived(object sender, MsgReceivedEventArgs <TargetQueryMessage> e)
{
int robotIDRequested = e.message.RobotID; // this would be just zero... now let's reassign this.
Bitmap gg;
if (robotIDToPackage.Count == 0) // send anything. The robot ID would be zero.
//if (!robotIDToPackage.ContainsKey(robotIDRequested))
{
Console.WriteLine("No detection found yet");
string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\noDetection.png";
gg = new Bitmap(fileLocation);
RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0);
targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1, TargetTypes.Junk));
return;
}
int targetID = 0;
//robotIDRequested = (int)Math.Ceiling(r.NextDouble() * robotIDToPackage.Count);
//List<int> robotIDList = robotIDToPackage.Keys.ToList();
robotIDRequested = robotIDToPackage.Keys.ToList()[(int)Math.Floor(r.NextDouble() * robotIDToPackage.Count)];
RobotImage img = robotIDToPackage[robotIDRequested].GetUnconfirmedRobotImage(ref targetID, missingTargets);
if (targetID == 0)
{
Console.WriteLine("TargetID is not valid.");
}
if (img == null)
{
Console.WriteLine("No Image Queue to process OR no more unconfirmed image");
string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\allConfirmed.png";
gg = new Bitmap(fileLocation);
RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0);
targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1, TargetTypes.Junk));
return;
}
else
{
gg = new Bitmap(img.image.Clone() as Image);
}
//if (targetIDToInformation[targetID].targetCov[0, 0] > 0.5 && targetIDToInformation[targetID].targetCov[1, 1] > 0.5)
//{
// Console.WriteLine("No Good Target with small covariance");
// string fileLocation = Directory.GetParent(Directory.GetParent(Directory.GetCurrentDirectory()).ToString()) + "\\noGoodTarget.png";
// gg = new Bitmap(fileLocation);
// RobotImage noImg = new RobotImage(robotIDRequested, gg, 0.0);
// targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, noImg, 0.0, -1));
// return;
//}
Graphics g = Graphics.FromImage(gg);
Vector2 RBCorner = robotIDToPackage[robotIDRequested].pixelRBCorner[targetID];
Vector2 LTCorner = robotIDToPackage[robotIDRequested].pixelLTCorner[targetID];
//if (e.message.RobotID == 1)
g.DrawRectangle(new Pen(Color.Red, 3.0f), new Rectangle((int)(LTCorner.X / 2 * 1) - 10, (int)((LTCorner.Y / 2 * 1) - 10),
(int)(RBCorner.X / 2 - LTCorner.X / 2) * 1 + 10, (int)((RBCorner.Y - LTCorner.Y) / 2 * 1 + 10)));
/*
* else
* g.DrawRectangle(new Pen(Color.Red, 3.0f), new Rectangle((int)LTCorner.X - 10, (int)LTCorner.Y - 10,
* (int)(RBCorner.X - LTCorner.X) + 10, (int)(RBCorner.Y - LTCorner.Y) + 10));
*/
Bitmap toSend = new Bitmap(160, 40);
Graphics gr = Graphics.FromImage(toSend);
gr.DrawImage(gg, 0, 0, 160, 40);
RobotImage rImg = new RobotImage(img.robotID, toSend, img.timeStamp);
targetListServer.SendUnreliably(new TargetListMessage(robotIDRequested, rImg, robotIDToPackage[robotIDRequested].timeStamp[targetID], targetID, targetIDToInformation[targetID].targetState.ToVector2(), targetIDToInformation[targetID].targetCov, targetIDToInformation[targetID].type));
detectionStateScanServer.SendUnreliably(new LidarPoseTargetMessage(robotIDRequested, targetID, targetIDToInformation[targetID].detectedPose, robotIDToPackage[robotIDRequested].lidarScan[targetID]));
}
void UpdateDetectedPicture(Object o)
{
int count = 0;
while (isRunning)
{
Dictionary <int, TargetListPackage> copy = new Dictionary <int, TargetListPackage>(robotIDToPackage);
foreach (KeyValuePair <int, TargetListPackage> pair in copy)
{
Dictionary <int, double> target2tsDict;
lock (locker)
{ target2tsDict = new Dictionary <int, double>(pair.Value.timeStamp); }
foreach (KeyValuePair <int, double> target2ts in target2tsDict)
{
// make sure the detection time is already within the image queue
double ts = imageQueue.GetMostRecentTimestamp(pair.Key);
if (ts != -1 && target2ts.Value < ts)
{
RobotImage img = imageQueue.QueueClosestImage(pair.Key, target2ts.Value);
if (img != null)
{
pair.Value.robotImage[target2ts.Key] = img;
}
}
}
}
// existing targets association
Dictionary <int, TargetInformation> dict = new Dictionary <int, TargetInformation>();
if (targetIDToInformation.Count != 0)
{
//dict.Add(targetIDToInformation.Keys.ToList()[0], targetIDToInformation.Values.ToList()[0]);
Dictionary <int, TargetInformation> targetIDInformationCopy;
lock (locker)
{
targetIDInformationCopy = new Dictionary <int, TargetInformation>(targetIDToInformation);
}
foreach (KeyValuePair <int, TargetInformation> pair in targetIDInformationCopy)
{
// find missing targets
if (!targetIDfromTracker.Contains(pair.Key) && targetIDfromTracker.Count > 0 && !missingTargets.Contains(pair.Key))
{
missingTargets.Add(pair.Key);
}
if (targetIDfromTracker.Contains(pair.Key) && missingTargets.Contains(pair.Key))
{
missingTargets.Remove(pair.Key);
}
int dictLength = dict.Count; bool isWithinRange = false;
int i = 0;
for (i = 0; i < dictLength; i++) // go through the new list creating and check the distance
{
if (dict[dict.Keys.ToList()[i]].Equals(pair.Value) || missingTargets.Contains(pair.Key)) // if you're looking at the same object, ignore
{
continue;
}
double dist = dict[dict.Keys.ToList()[i]].targetState.ToVector2().DistanceTo(pair.Value.targetState.ToVector2());
if (dist < 0.5 || dist == 0)
{
isWithinRange = true;
break; // if you find anything within this range, break and skip this thing (do not add to the list)
}
}
if (!isWithinRange)
{
dict.Add(pair.Key, pair.Value);
}
else // if the target is inside the range...
{
if (!associationDictionary.ContainsKey(pair.Key))
{
associationDictionary.Add(pair.Key, dict.Keys.ToList()[i]); // make a dictionary that tells which targetID is which targetID
}
// from another robot.
}
}
associatedTargetID2Information = dict;
}
if (count >= 10)
{
if (associatedTargetID2Information.Count != 0)
{
List <RobotPose> targetState = new List <RobotPose>(), detectedPose = new List <RobotPose>();
List <int> targetID = new List <int>();; List <Matrix> targetCov = new List <Matrix>(); List <TargetTypes> ooiTypes = new List <TargetTypes>();
foreach (KeyValuePair <int, TargetInformation> pair in associatedTargetID2Information)
{
if (pair.Value.type != TargetTypes.Junk)
{
targetState.Add(pair.Value.targetState);
targetCov.Add(pair.Value.targetCov);
targetID.Add(pair.Key);
detectedPose.Add(pair.Value.detectedPose);
ooiTypes.Add(pair.Value.type);
}
}
targetToCentralServer.SendUnreliably(new TargetList2CentralNodeMessage(targetState, targetCov, targetID, detectedPose, ooiTypes));
unconfirmedTargetNoServer.SendUnreliably(new UnconfirmedTargetNumberMessage(FindUnconfirmedTargetNumber(associatedTargetID2Information.Values.ToList()) - missingTargets.Count));
count = 0;
}
else
{
List <RobotPose> targetState = new List <RobotPose>(), detectedPose = new List <RobotPose>();
List <int> targetID = new List <int>();; List <Matrix> targetCov = new List <Matrix>(); List <TargetTypes> ooiTypes = new List <TargetTypes>();
targetToCentralServer.SendUnreliably(new TargetList2CentralNodeMessage(targetState, targetCov, targetID, detectedPose, ooiTypes));
unconfirmedTargetNoServer.SendUnreliably(new UnconfirmedTargetNumberMessage(FindUnconfirmedTargetNumber(associatedTargetID2Information.Values.ToList()) - missingTargets.Count));
count = 0;
}
}
count++;
Thread.Sleep(10);
}
}
