public void OnLidarProcessedLineReceived(object sender, List <SegmentExtended> list_of_segments)
{
List <SegmentExtended> corrected_list_segment = new List <SegmentExtended>();
foreach (SegmentExtended segment in list_of_segments)
{
PolarPointRssi point_a = Toolbox.ConvertPointDToPolar(new PointD(segment.Segment.X1, segment.Segment.Y1));
PolarPointRssi point_b = Toolbox.ConvertPointDToPolar(new PointD(segment.Segment.X2, segment.Segment.Y2));
PointD correct_point_a = new PointD(
RobotLocation.X + (point_a.Distance * Math.Cos(RobotLocation.Theta + point_a.Angle)),
RobotLocation.Y + (point_a.Distance * Math.Sin(RobotLocation.Theta + point_a.Angle))
);
PointD correct_point_b = new PointD(
RobotLocation.X + (point_b.Distance * Math.Cos(RobotLocation.Theta + point_b.Angle)),
RobotLocation.Y + (point_b.Distance * Math.Sin(RobotLocation.Theta + point_b.Angle))
);
corrected_list_segment.Add(new SegmentExtended(correct_point_a, correct_point_b, segment.Color, segment.Width));
}
LidarSegment = corrected_list_segment;
OnLocalWorldMapEvent?.Invoke(this, this);
}
private void ProcessReceivedData(TiMDataMessage TimData)
{
lock (LidarPoints)
{
LidarPoints.Clear();
LidarPolarPointsList.Clear();
double angleLidar = Toolbox.DegToRad(Location.Theta);
for (int i = 0; i < TimData.AngleData.Count; i++)
{
if (TimData.AngleData[i] >= AngleMin && TimData.AngleData[i] <= AngleMax) // Filtre d'angle
{
double distance = TimData.DistanceData[i] / 1000.0;
double angle = Toolbox.DegToRad(TimData.AngleData[i]);
//On trouve les coordonnées du point brut en cartésien dans le ref du Lidar
double xRefRobot = Location.X + distance * Math.Cos(angle + angleLidar);
double yRefRobot = Location.Y + distance * Math.Sin(angle + angleLidar);
//On trouve les coordonnées du point en polaire dans le ref du robot
double distanceCentreRobot =
Math.Sqrt(Math.Pow(xRefRobot, 2) +
Math.Pow(yRefRobot, 2));
double angleAxeRobot;
if (yRefRobot != 0 && xRefRobot != 0)
{
angleAxeRobot = Math.Atan2(yRefRobot, xRefRobot);
}
else
{
angleAxeRobot = angle;
}
PolarPointRssi pt = new PolarPointRssi(angleAxeRobot, distance, TimData.RssiData[i]);
//LidarPoint point = new LidarPoint(distanceCentreRobot, angleAxeRobot, TimData.RssiData[i]);
// Supression des points en dehors des limites hardware
//if (distance >= 0.01 && distance <= 10.01) // Filtre de distance
{
//LidarPoints.Add(point);
LidarPolarPointsList.Add(pt);
}
}
}
}
//OnLidarPointsReady();
OnLidarDecodedFrame(robotId, LidarPolarPointsList);
}
public Cup DetectCup(ClusterObjects cluster)
{
/// TEMPORARY NEED TO EDIT: ONLY FOR DEBUG PURPOSE
PolarPointRssi begin_point = cluster.points[0].Pt;
PolarPointRssi end_point = cluster.points[cluster.points.Count - 1].Pt;
double lenght_of_cluster = Toolbox.Distance(begin_point, end_point);
if (lenght_of_cluster >= 0.040 && lenght_of_cluster <= 0.08)
{
List <PointD> pointDs = cluster.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList();
double median = 0.80;
double b = cluster.points[(int)(cluster.points.Count() * median)].Pt.Rssi;
double e = cluster.points[(int)(cluster.points.Count() * (1 - median))].Pt.Rssi;
double moyenne = (b + e) / 2;
Color color = Color.White;
if (moyenne >= 9000 && moyenne <= 12000)
{
color = Color.Green;
}
else if (moyenne >= 12000 && moyenne <= 14000)
{
color = Color.Red;
}
else
{
return(new Cup());
}
//Console.WriteLine(moyenne);
PointD center_point = GetMediumPoint(pointDs);
return(new Cup(center_point, 0.065, color));
}
else
{
return(null);
}
}
public List <ClusterObjects> DetectClusterOfPoint(List <PolarPointRssi> pointsList, double thresold, int mininum_amount_of_points)
{
/// ABD Stand for Adaptative breakpoint Detection
List <ClusterObjects> listOfClusters = new List <ClusterObjects>();
ClusterObjects cluster = new ClusterObjects();
int i;
for (i = 1; i < pointsList.Count - 1; i++)
{
PolarPointRssi point_n_minus_1 = pointsList[i - 1];
PolarPointRssi point_n_plus_1 = pointsList[i + 1];
PolarPointRssi point_n = pointsList[i];
double dist_n_minus_1 = point_n_minus_1.Distance;
double delta_theta = Math.Abs(point_n_minus_1.Angle - point_n.Angle);
double lambda = point_n_plus_1.Angle - point_n_minus_1.Angle;
double ABD_Thresold = thresold; // dist_n_minus_1 * (Math.Sin(delta_theta) / Math.Sin(lambda - delta_theta));
double distance_between_point = Toolbox.Distance(point_n, point_n_minus_1);
if (distance_between_point < ABD_Thresold)
{
cluster.points.Add(new PolarPointRssiExtended(point_n, 3, Color.Purple));
}
else
{
if (cluster.points.Count() > mininum_amount_of_points)
{
listOfClusters.Add(cluster);
}
cluster = new ClusterObjects();
}
}
if (cluster.points.Count() > mininum_amount_of_points)
{
listOfClusters.Add(cluster);
}
return(listOfClusters);
}
private List <LidarDetectedObject> DetectionBalisesCatadioptriquesParRssiEtTaille(List <PolarPointRssi> ptList, double distanceMax)
{
double minRssiBalise = 60;
List <LidarDetectedObject> BalisesCatadioptriquesList = new List <LidarDetectedObject>();
LidarDetectedObject currentObject = new LidarDetectedObject();;
List <PolarPointRssi> BalisePointList = new List <PolarPointRssi>();
////Détection des objets ayant un RSSI dans un intervalle correspondant aux catadioptres utilisés et proches
////double maxRssiCatadioptre = 90;
//double minRssiCatadioptre = 60;
//var selectedPoints = ptList.Where(p => (p.Rssi >= minRssiCatadioptre) && (p.Distance < distanceMax));
//List<PolarPointRssi> balisesPointsList = (List<PolarPointRssi>)selectedPoints.ToList();
//On détecte les max de RSSI supérieurs à un minRssiBalise
List <int> maxRssiIndexList = new List <int>();
if (ptList.Count() > 0)
{
for (int i = 1; i < ptList.Count - 1; i++)
{
if (ptList[i].Rssi >= ptList[i - 1].Rssi && ptList[i].Rssi > ptList[i + 1].Rssi && ptList[i].Rssi > minRssiBalise)
{
maxRssiIndexList.Add(i);
}
}
//Gestion des cas de bord de tableau pour ne pas avoir de zone morte
if (ptList[0].Rssi >= ptList[ptList.Count - 1].Rssi && ptList[0].Rssi > ptList[1].Rssi && ptList[0].Rssi > minRssiBalise)
{
maxRssiIndexList.Add(0);
}
if (ptList[ptList.Count - 1].Rssi >= ptList[ptList.Count - 2].Rssi && ptList[ptList.Count - 1].Rssi > ptList[0].Rssi && ptList[ptList.Count - 1].Rssi > minRssiBalise)
{
maxRssiIndexList.Add(ptList.Count - 1);
}
}
//ON génère la liste des points des balises pour affichage de debug uniquement
BalisePointList = new List <PolarPointRssi>();
for (int i = 0; i < ptList.Count; i++)
{
PolarPointRssi pt = new PolarPointRssi(ptList[i].Angle, 0, ptList[i].Rssi);
BalisePointList.Add(pt);
}
//On regarde la taille des objets autour des max de Rssi
double seuilSaillance = 0.2;
foreach (int indexPicRssi in maxRssiIndexList)
{
//On cherche à détecter les fronts montants de distance (distance qui augmente brutalement) autour des pics pour déterminer la taille des objets
//On définit une taille max de recherche correspondant à 5 fois la taille d'une balise
double tailleAngulaireBalisePotentielle = 0.15 / ptList[indexPicRssi].Distance;
double incrementAngleLidar = ptList[1].Angle - ptList[0].Angle;
//Défini la fenetre de recherche
int indexSearchWindow = (int)(tailleAngulaireBalisePotentielle / incrementAngleLidar);
int indexFrontMontantAngleSup = -1;
int indexFrontMontantAngleInf = -1;
//Détection des fronts montants coté des angles inférieurs à l'angle central
int index = indexPicRssi;
int indexShift = 0; //Pour gérer les balises situées à l'angle 0
while (index > indexPicRssi - indexSearchWindow)
{
if (index - 1 < 0)
{
indexShift = ptList.Count(); //Gestion des bords de tableau
}
BalisePointList[index + indexShift - 1].Distance = 5; //For debug display
if (Math.Abs(ptList[index + indexShift - 1].Distance - ptList[indexPicRssi].Distance) > seuilSaillance)
{
//On a un front montant coté des angles inférieurs à l'angle central
indexFrontMontantAngleInf = index + indexShift - 1;
break;
}
index--;
}
//Détection des fronts montants coté des angles supérieurs à l'angle central
index = indexPicRssi;
indexShift = 0; //Pour gérer les balises situées à l'angle 0
while (index < indexPicRssi + indexSearchWindow)
{
if (index + 1 >= ptList.Count)
{
indexShift = -ptList.Count(); //Gestion des bords de tableau
}
BalisePointList[index + indexShift + 1].Distance = 6; //For debug display
if (Math.Abs(ptList[index + indexShift + 1].Distance - ptList[indexPicRssi].Distance) > seuilSaillance)
{
//On a un front montant coté des angles supérieurs à l'angle central
indexFrontMontantAngleSup = index + indexShift + 1;
break;
}
index++;
}
if (indexFrontMontantAngleInf >= 0 && indexFrontMontantAngleSup >= 0)
{
//On a deux fronts montants de part et d'autre du pic, on regarde la taille de l'objet
double tailleObjet = (ptList[indexFrontMontantAngleSup].Angle - ptList[indexFrontMontantAngleInf].Angle)
* (ptList[indexFrontMontantAngleSup].Distance + ptList[indexFrontMontantAngleInf].Distance + ptList[indexPicRssi].Distance) / 3;
//if(tailleObjet>0.05&& tailleObjet<0.25)
{
//On a probablement un catadioptre de type balise Eurobot !
currentObject = new LidarDetectedObject();
if (indexFrontMontantAngleInf < indexFrontMontantAngleSup)
{
for (int i = indexFrontMontantAngleInf + 1; i < indexFrontMontantAngleSup - 1; i++) //Décalages pour éviter de mettre une distance erronnée dans les amas de points
{
currentObject.PtList.Add(ptList[i]);
BalisePointList[i].Distance = 10; //For debug display
}
currentObject.ExtractObjectAttributes();
BalisesCatadioptriquesList.Add(currentObject);
}
else //Gestion des objets coupés en deux par l'angle 0
{
for (int i = indexFrontMontantAngleInf + 1; i < ptList.Count; i++) //Décalages pour éviter de mettre une distance erronnée dans les amas de points
{
currentObject.PtList.Add(ptList[i]);
BalisePointList[i].Distance = 10; //For debug display
}
for (int i = 0; i < indexFrontMontantAngleSup - 1; i++) //Décalages pour éviter de mettre une distance erronnée dans les amas de points
{
currentObject.PtList.Add(new PolarPointRssi(ptList[i].Angle + 2 * Math.PI, ptList[i].Distance, ptList[i].Rssi));
BalisePointList[i].Distance = 10; //For debug display
}
currentObject.ExtractObjectAttributes();
BalisesCatadioptriquesList.Add(currentObject);
}
}
}
}
OnLidarBalisePointListForDebug(robotId, BalisePointList);
return(BalisesCatadioptriquesList);
}
