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;
                    }
                }
            }
        }
Esempio n. 2
0
        void ocGridLogOdd_NewGridAvailable(object sender, Magic.Common.Sensors.NewOccupancyGrid2DAvailableEventArgs e)
        {
            ocGrid2Poly.UpdateOccupancyGrid(e.OccupancyGrid);
            List <Polygon> polyList = ocGrid2Poly.FindPolygons();

            globalObstacleServer.SendUnreliably(new GlobalObstaclesMessage(polyList));
        }
Esempio n. 3
0
        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");

            //}
        }
Esempio n. 6
0
 public void SendLocalMapRequest(RobotPose currentPose, double extentX, double extentY)
 {
     localMapRequestServer.SendUnreliably(new LocalMapRequestMessage(robotID, currentPose, extentX, extentY));
 }
Esempio n. 7
0
        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]));
        }
Esempio n. 8
0
        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);
            }
        }