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