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