Beispiel #1
0
 public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
 {
     //Segmentation en objets
     if (robotId == e.RobotId)
     {
         ProcessLidarData(e.AngleList, e.DistanceList);
     }
 }
        public void OnRawLidarArgs(object sender, RawLidarArgs rawLidarArgs)
        {
            LidarFrame++;

            OnRawLidarDataEvent?.Invoke(this, rawLidarArgs);
            OnRawLidarPointPolarEvent?.Invoke(this, rawLidarArgs.PtList.Select(x => new PolarPointRssiExtended(x, 3, Color.Purple)).ToList());
            OnRawLidarPointXYEvent?.Invoke(this, rawLidarArgs.PtList.Select(x => new PointDExtended(Toolbox.ConvertPolarToPointD(x), Color.Blue, 2)).ToList());
            ProcessLidarData(rawLidarArgs.PtList);
        }
Beispiel #3
0
        public virtual void OnLidarProcessedData(object sender, RawLidarArgs e)
        {
            var handler = OnLidarProcessedDataEvent;

            if (handler != null)
            {
                handler(this, e);
            }
        }
Beispiel #4
0
        public virtual void OnLidarRawData(RawLidarArgs e)
        {
            var handler = OnLidarRawDataEvent;

            if (handler != null)
            {
                handler(this, e);
            }
        }
 public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
 {
     //Segmentation en objets
     if (robotId == e.RobotId)
     {
         ProcessLidarData(e.PtList);
         EvaluateSpeed(e.PtList);
     }
 }
Beispiel #6
0
        public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
        {
            List <Point> ptList = new List <Point>();

            ptList = e.PtList.Select(p => new Point(p.Angle, p.Rssi)).ToList();
            oscilloLidar.UpdatePointListOfLine(0, ptList);
            List <Point> ptList2 = new List <Point>();

            ptList2 = e.PtList.Select(p => new Point(p.Angle, p.Distance)).ToList();
            oscilloLidar.UpdatePointListOfLine(1, ptList2);
        }
Beispiel #7
0
        public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
        {
            RawLidarArgsLog data = new RawLidarArgsLog();

            data.PtList      = e.PtList;
            data.RobotId     = e.RobotId;
            data.InstantInMs = DateTime.Now.Subtract(initialDateTime).TotalMilliseconds;
            var msg = ZeroFormatterSerializer.Serialize <ZeroFormatterLogging>(data);

            Log(msg);
        }
Beispiel #8
0
        public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
        {
            RawLidarArgsWithTimeStamp data = new RawLidarArgsWithTimeStamp();

            data.AngleList    = e.AngleList;
            data.DistanceList = e.DistanceList;
            data.RobotId      = e.RobotId;
            data.InstantInMs  = DateTime.Now.Subtract(initialDateTime).TotalMilliseconds;
            string json = JsonConvert.SerializeObject(data);

            Log(json);
        }
        public void OnRawPointAvailable(object sender, LidarPointsReadyEventArgs lidarPoints)
        {
            RawLidarArgs rawLidar = new RawLidarArgs()
            {
                RobotId          = robotId,
                LidarFrameNumber = LidarFrame,
                PtList           = lidarPoints.LidarPoints.Select(x => new PolarPointRssi(x.Angle, x.Distance, x.RSSI)).ToList()
            };


            OnRawLidarArgs(sender, rawLidar);
        }
Beispiel #10
0
        public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
        {
            //On forward si on n'est pas en mode replay
            //if (!replayModeActivated)
            {
                //On forward les données lidar brutes reçues au lidarProcessor
                lidarProcessor.OnRawLidarDataReceived(sender, e);
                //On rémet un event pour les affichages éventuels
                OnLidarRawData(e);

                //On génère la perception
                GeneratePerception();
            }
        }
Beispiel #11
0
        public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
        {
            //On forward si on n'est pas en mode replay
            //if (!replayModeActivated)
            {
                //On forward les données lidar brutes reçues au lidarProcessor
                lidarProcessor.OnRawLidarDataReceived(sender, e);
                //On rémet un event pour les affichages éventuels
                OnLidarRawData(e);


                /////////////////////////////////////////////////////////////////////////////////////////////////
                //TODO : remove that lines - just there for using recording with Lidar only
                //On génère la perception
                GeneratePerception();

                /////////////////////////////////////////////////////////////////////////////////////////////////
            }
        }
Beispiel #12
0
 public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
 {
     LidarPtList = e.PtList;
 }
        public void ProcessLidarData(List <PolarPointRssi> polarPointRssi)
        {
            List <SegmentExtended> Lines = new List <SegmentExtended>();

            List <PolarPointRssi> validPoint   = polarPointRssi.Where(x => x.Distance <= Math.Sqrt(Math.Pow(3, 2) + Math.Pow(2, 2))).ToList();
            List <PointD>         validPointXY = validPoint.Select(x => Toolbox.ConvertPolarToPointD(x)).ToList();

            List <PointDExtended> absolutePoints = new List <PointDExtended>();

            validPointXY = ClustersDetection.ExtractClusterByDBScan(validPointXY, 0.05, 3).SelectMany(x => x.points).ToList().Select(x => Toolbox.ConvertPolarToPointD(x)).ToList().Select(x => x.Pt).ToList();


            RectangleOriented     best_rectangle     = FindRectangle.FindMbrBoxByOverlap(validPointXY);
            List <PointD>         border_points      = FindRectangle.FindAllBorderPoints(validPointXY, best_rectangle, 0.05);
            List <PolarPointRssi> border_point_polar = border_points.Select(x => Toolbox.ConvertPointDToPolar(x)).ToList();

            List <ClusterObjects>         inside_clusters = ClustersDetection.ExtractClusterByDBScan(validPointXY.Where(x => border_points.IndexOf(x) == -1).ToList(), 0.045, 3);
            List <PolarPointRssiExtended> processedPoints = ClustersDetection.SetColorsOfClustersObjects(inside_clusters);


            List <ClusterObjects> border_clusters = ClustersDetection.ExtractClusterByDBScan(border_points, 0.05, 10);

            List <SegmentExtended> iepfs_lines = new List <SegmentExtended>();

            foreach (ClusterObjects c in border_clusters)
            {
                var iepf_border_points = LineDetection.IEPF_Algorithm(c.points.Select(x => Toolbox.ConvertPolarToPointD(x).Pt).ToList(), 0.05);

                for (int i = 1; i < iepf_border_points.Count; i++)
                {
                    iepfs_lines.Add(new SegmentExtended(iepf_border_points[i - 1], iepf_border_points[i], Color.Black, 2));
                }
            }



            iepfs_lines = LineDetection.MergeSegmentWithLSM(iepfs_lines, 0.5, 3 * Math.PI / 180);
            Lines.AddRange(iepfs_lines);

            List <List <SegmentExtended> > list_of_family = LineDetection.FindFamilyOfSegment(iepfs_lines);

            List <PointD> corners_points = CornerDetection.FindAllValidCrossingPoints(list_of_family).SelectMany(x => x).ToList().Select(x => x.Pt).ToList();
            Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(best_rectangle);

            double thresold = 0.09;

            double width  = Math.Max(best_rectangle.Lenght, best_rectangle.Width);
            double height = Math.Min(best_rectangle.Lenght, best_rectangle.Width);

            List <SegmentExtended> rectangle_segments = FindRectangle.DrawRectangle(best_rectangle, Color.Green, 1);

            //if (width >= 3 - thresold && height >= 2 - thresold)
            //{
            //    Console.ForegroundColor = ConsoleColor.Green;
            //}
            //else if (width >= 3 - thresold)
            //{
            //    Console.ForegroundColor = ConsoleColor.Blue;


            //}
            //else if (width > 2 + thresold)
            //{
            //    Console.ForegroundColor = ConsoleColor.Yellow;
            //    if (height > 2 - thresold)
            //        Console.ForegroundColor = ConsoleColor.DarkYellow;
            //}
            //else if (width >= 2 - thresold && width <= 2 + thresold)
            //    Console.ForegroundColor = ConsoleColor.Red;
            //else
            //    Console.ResetColor();

            //Console.WriteLine(width + " " + height);

            processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(best_rectangle.Center), 10, Color.Black));


            Lines.AddRange(rectangle_segments);

            //Console.WriteLine("Corners: " + FindRectangle.GetNumberOfVisibleCorners(best_rectangle));

            RectangleOriented resized_rectangle = FindRectangle.ResizeRectangle(best_rectangle, thresold);
            List <Location>   list_of_possible_locations;

            if (resized_rectangle != null)
            {
                Console.ForegroundColor = ConsoleColor.Green;
                Lines.AddRange(FindRectangle.DrawRectangle(resized_rectangle, Color.LightGreen, 8));
                processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(resized_rectangle.Center), 10, Color.Green));

                list_of_possible_locations = FindRectangle.ListAllPossibleLocation(resized_rectangle);
                //processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red : Color.DarkRed)).ToList());
            }
            else
            {
                Tuple <RectangleOriented, RectangleOriented, RectangleOriented> list_of_possible_rectangles = FindRectangle.ListResisableRectangle(best_rectangle, thresold);
                Console.ForegroundColor    = ConsoleColor.Yellow;
                list_of_possible_locations = FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item2);

                //Console.ForegroundColor = ConsoleColor.Blue;
                //list_of_possible_locations.AddRange(FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item2));

                //Console.ForegroundColor = ConsoleColor.Red;
                //list_of_possible_locations.AddRange(FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item3));


                //Console.ForegroundColor = ConsoleColor.Red;
                //Console.WriteLine("1 ------");

                //processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red : Color.DarkRed)).ToList());

                //Console.ForegroundColor = ConsoleColor.Yellow;
                //Console.WriteLine("2 ------");


                Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item1, Color.Yellow, 4));
                //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item1.Center), 10, Color.Yellow));
                Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item2, Color.Blue, 4));
                //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item2.Center), 10, Color.Blue));
                Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item3, Color.Red, 4));
                //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item3.Center), 10, Color.LightGoldenrodYellow));
            }
            //processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red: Color.DarkBlue)).ToList());

            absolutePoints = list_of_possible_locations.Select(x => new PointDExtended(new PointD(x.X, x.Y), x.Theta > 0 ? Color.Red : Color.Blue, 10)).ToList();
            Location best_location = FindRectangle.GetBestLocation(list_of_possible_locations, robotLocation);

            //processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(best_location.X, best_location.Y)), 10, Color.DarkGreen));
            OnLidarSetupRobotLocationEvent?.Invoke(this, best_location);
            Console.ResetColor();



            List <Cup>          list_of_cups    = new List <Cup>();
            List <LidarObjects> list_of_objects = new List <LidarObjects>();

            foreach (ClusterObjects c in inside_clusters)
            {
                RectangleOriented cluster_rectangle = FindRectangle.FindMbrBoxByArea(c.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList());
                cluster_rectangle.Width  += 0.1;
                cluster_rectangle.Lenght += 0.1;

                //Lines.AddRange(FindRectangle.DrawRectangle(cluster_rectangle, c.points[0].Color, 3));

                Color color = Toolbox.ColorFromHSL((list_of_objects.Count * 0.20) % 1, 1, 0.5);
                list_of_objects.Add(new LidarObjects(c.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList(), color));


                Cup cup = DetectCup(c);
                // The null condition is Bad need to edit
                if (cup != null)
                {
                    list_of_cups.Add(cup);
                }
            }

            OnProcessLidarObjectsDataEvent?.Invoke(this, list_of_objects);

            // Lines.Add(DetectGlobalLine(polarPointRssi, 1d, 0d, 5d, 3, 0.2d));
            OnProcessLidarLineDataEvent?.Invoke(this, Lines);

            OnProcessLidarCupDataEvent?.Invoke(this, list_of_cups);


            RawLidarArgs processLidar = new RawLidarArgs()
            {
                RobotId = robotId, LidarFrameNumber = LidarFrame, PtList = processedPoints.Select(x => x.Pt).ToList()
            };

            OnProcessLidarDataEvent?.Invoke(this, processLidar);

            OnProcessLidarPolarDataEvent?.Invoke(this, processedPoints);
            OnProcessLidarAbsoluteDataEvent?.Invoke(this, absolutePoints);
            //OnProcessLidarXYDataEvent?.Invoke(this, processedPoints.Select(x => new PointDExtended(Toolbox.ConvertPolarToPointD(x), Color.Blue, 2)).ToList());
        }
Beispiel #14
0
 private void OnLidarBalisePointListForDebugReceived(object sender, RawLidarArgs e)
 {
     //On transmet l'event
     OnLidarBalisePointListForDebug(e.RobotId, e.PtList);
 }
Beispiel #15
0
 public virtual void OnLidarRawData(RawLidarArgs e)
 {
     OnLidarRawDataEvent?.Invoke(this, e);
 }
 public void OnRawLidarDataReceived(object sender, RawLidarArgs e)
 {
     //Fonctions de traitement
 }
        public void OnRobotLidarReceived(object sender, RawLidarArgs rawLidar)
        {
            LIDAR_ANGLE_RESOLUTION = Math.Abs(rawLidar.PtList[0].Angle - rawLidar.PtList[1].Angle);

            ExtractLineLandmarks(rawLidar.PtList, RobotLocation);
        }
Beispiel #18
0
 public void OnProcessLidarDataReceived(object sender, RawLidarArgs e)
 {
     //List<Point> ptListProcess = new List<Point>();
     //ptListProcess = e.PtList.Select(p => new Point(p.Angle, p.Distance)).ToList();
     //oscilloLidar.UpdatePointListOfLine(2, ptListProcess);
 }