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); }
public virtual void OnLidarProcessedData(object sender, RawLidarArgs e) { var handler = OnLidarProcessedDataEvent; if (handler != null) { handler(this, e); } }
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); } }
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); }
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); }
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); }
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(); } }
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(); ///////////////////////////////////////////////////////////////////////////////////////////////// } }
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()); }
private void OnLidarBalisePointListForDebugReceived(object sender, RawLidarArgs e) { //On transmet l'event OnLidarBalisePointListForDebug(e.RobotId, e.PtList); }
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); }
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); }