void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs <LocalMapRequestMessage> e)
{
if (e.message == null)
{
return;
}
UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, gaussianMixMapAlgorithm.UhatGM, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY);
if (mapToSend != null)
{
int segmentLength = 2000;
int numIteration = mapToSend.CellData.Count / segmentLength;
int numLeftOver = mapToSend.CellData.Count - (numIteration * segmentLength);
int lastIndex = 0;
for (int i = 0; i < numIteration + 1; i++)
{
if (i == numIteration)
{
localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, numLeftOver)));
}
else
{
localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, segmentLength)));
lastIndex += segmentLength;
}
}
}
}
void ocGridLogOdd_NewGridAvailable(object sender, Magic.Common.Sensors.NewOccupancyGrid2DAvailableEventArgs e)
{
ocGrid2Poly.UpdateOccupancyGrid(e.OccupancyGrid);
List <Polygon> polyList = ocGrid2Poly.FindPolygons();
globalObstacleServer.SendUnreliably(new GlobalObstaclesMessage(polyList));
}
static void poseClient_MsgReceived(object sender, MsgReceivedEventArgs <RobotPoseMessage> e)
{
if (e.message == null)
{
return;
}
pose = e.message.Pose;
protoPose.x = pose.x;
protoPose.y = pose.y;
protoPose.z = pose.z;
protoPose.yaw = pose.yaw;
protoPose.pitch = pose.pitch;
protoPose.roll = pose.roll;
protoPose.timeStamp = pose.TimeStamp;
protoPoseServer.SendUnreliably(protoPose);
// Clear the screen
Console.Clear();
Console.SetCursorPosition(Console.CursorLeft, Console.CursorTop);
Console.WriteLine("Robot pose:");
Console.WriteLine("X:\t" + pose.x);
Console.WriteLine("Y:\t" + pose.y);
Console.WriteLine("Z:\t" + pose.z);
Console.WriteLine("yaw:\t" + pose.yaw);
Console.WriteLine("pitch:\t" + pose.pitch);
Console.WriteLine("roll:\t" + pose.roll);
Console.WriteLine("timestamp:\t" + pose.timestamp);
}
void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs <LocalMapRequestMessage> e)
{
if (e.message == null)
{
return;
}
UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, globalOcGrid, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY);
localMapReponseServer.SendUnreliably(mapToSend);
}
void SendGlobalUpdate()
{
//while (isRunning)
//{
List <Index> indexList; List <float> heightList; List <float> covList; List <float> pijSumList; List <float> laserHitList;
gaussianMixMapAlgorithm.GetArraysToSend(out indexList, out heightList, out covList, out pijSumList, out laserHitList);
if (indexList.Count == 0)
{
return;
}
int lastIndex = 0;
int segmentLength = 3000;
int numIteration = indexList.Count / segmentLength;
int numLeftOver = indexList.Count - (numIteration * segmentLength);
Stopwatch sw = new Stopwatch();
sw.Start();
for (int i = 0; i < numIteration + 1; i++)
{
if (i == numIteration)
{
globalGaussianMixMapServer.SendUnreliably(new GlobalMapCell(globalOcGrid.ResolutionX, globalOcGrid.ExtentX, indexList.GetRange(lastIndex, numLeftOver),
heightList.GetRange(lastIndex, numLeftOver), covList.GetRange(lastIndex, numLeftOver), pijSumList.GetRange(lastIndex, numLeftOver), laserHitList.GetRange(lastIndex, numLeftOver)));
}
else
{
globalGaussianMixMapServer.SendUnreliably(new GlobalMapCell(globalOcGrid.ResolutionX, globalOcGrid.ExtentX, indexList.GetRange(lastIndex, segmentLength),
heightList.GetRange(lastIndex, segmentLength), covList.GetRange(lastIndex, segmentLength), pijSumList.GetRange(lastIndex, segmentLength), laserHitList.GetRange(lastIndex, segmentLength)));
lastIndex += segmentLength;
}
//Thread.Sleep(100);
}
Console.WriteLine("Sending time: " + sw.ElapsedMilliseconds + " ms");
//}
}
public void SendLocalMapRequest(RobotPose currentPose, double extentX, double extentY)
{
localMapRequestServer.SendUnreliably(new LocalMapRequestMessage(robotID, currentPose, extentX, extentY));
}
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);
}
}