void ProcessLidarData(List <PolarPointRssi> ptList)
{
List <LidarDetectedObject> ObjetsSaillantsList = new List <LidarDetectedObject>();
List <LidarDetectedObject> BalisesCatadioptriqueList = new List <LidarDetectedObject>();
List <LidarDetectedObject> BalisesCatadioptriqueList2 = new List <LidarDetectedObject>();
List <LidarDetectedObject> ObjetsProchesList = new List <LidarDetectedObject>();
List <LidarDetectedObject> ObjetsFondList = new List <LidarDetectedObject>();
List <LidarDetectedObject> ObjetsPoteauPossible = new List <LidarDetectedObject>();
LidarDetectedObject currentObject = new LidarDetectedObject();
//Opérations de traitement du signal LIDAR
//ptList = PrefiltragePointsIsoles(ptList, 0.04);
//ptList = MedianFilter(ptList, 5);
// BalisesCatadioptriqueList = DetectionBalisesCatadioptriques(ptList, 3.6);
BalisesCatadioptriqueList2 = DetectionBalisesCatadioptriquesParRssiEtTaille(ptList, 3.6);
ObjetsProchesList = DetectionObjetsProches(ptList, 0.17, 2.0, 0.2);
//Affichage des résultats
List <PolarPointListExtended> objectList = new List <PolarPointListExtended>();
foreach (var obj in ObjetsProchesList)
{
PolarPointListExtended currentPolarPointListExtended = new PolarPointListExtended();
currentPolarPointListExtended.polarPointList = new List <PolarPointRssi>();
{
currentPolarPointListExtended.polarPointList.Add(new PolarPointRssi(obj.AngleMoyen, obj.DistanceMoyenne, 0));
currentPolarPointListExtended.type = ObjectType.Obstacle;
objectList.Add(currentPolarPointListExtended);
}
}
foreach (var obj in BalisesCatadioptriqueList)
{
PolarPointListExtended currentPolarPointListExtended = new PolarPointListExtended();
currentPolarPointListExtended.polarPointList = new List <PolarPointRssi>();
{
currentPolarPointListExtended.polarPointList = obj.PtList;
currentPolarPointListExtended.type = ObjectType.Balise;
objectList.Add(currentPolarPointListExtended);
}
}
if (competition == GameMode.RoboCup)
{
///On fait la recherche du rectangle vide le plus grand
double tailleNoyau = 0.8;
//var ptListFiltered = Dilatation(Erosion(ptList, tailleNoyau), tailleNoyau);
//var ptListFiltered = Erosion(Dilatation(ptList, tailleNoyau), tailleNoyau);
//var ptListFiltered = Dilatation(ptList, tailleNoyau);
var ptListFiltered = Erosion(ptList, tailleNoyau);
//var ptListFiltered = ptList;
OnLidarProcessed(robotId, ptListFiltered);
}
OnLidarBalisesListExtracted(robotId, BalisesCatadioptriqueList2);
OnLidarObjectProcessed(robotId, objectList);
}
void ProcessLidarData(List <double> angleList, List <double> distanceList)
{
double zoomCoeff = 1.8;
List <double> AngleListProcessed = new List <double>();
List <double> DistanceListProcessed = new List <double>();
List <LidarDetectedObject> ObjetsSaillantsList = new List <LidarDetectedObject>();
List <LidarDetectedObject> ObjetsFondList = new List <LidarDetectedObject>();
List <LidarDetectedObject> ObjetsPoteauPossible = new List <LidarDetectedObject>();
LidarDetectedObject currentObject = new LidarDetectedObject();
//A enlever une fois le debug terminé
for (int i = 1; i < angleList.Count; i++)
{
distanceList[i] *= zoomCoeff;
}
//Préfiltrage des points isolés : un pt dont la distance aux voisin est supérieur à un seuil des deux coté est considere comme isolé.
double seuilPtIsole = 0.04 * zoomCoeff; //0.03 car le Lidar a une précision intrinsèque de +/- 1 cm.
for (int i = 1; i < angleList.Count - 1; i++)
{
if ((Math.Abs(distanceList[i - 1] - distanceList[i]) < seuilPtIsole) || (Math.Abs(distanceList[i + 1] - distanceList[i]) < seuilPtIsole))
{
AngleListProcessed.Add(angleList[i]);
DistanceListProcessed.Add(distanceList[i]);
}
}
angleList = AngleListProcessed;
distanceList = DistanceListProcessed;
//Détection des objets saillants
bool objetSaillantEnCours = false;
for (int i = 1; i < angleList.Count; i++)
{
//On commence un objet saillant sur un front descendant de distance
if (distanceList[i] - distanceList[i - 1] < -0.1 * zoomCoeff)
{
currentObject = new LidarDetectedObject();
currentObject.AngleList.Add(angleList[i]);
currentObject.DistanceList.Add(distanceList[i]);
objetSaillantEnCours = true;
}
//On termine un objet saillant sur un front montant de distance
if (distanceList[i] - distanceList[i - 1] > 0.15 * zoomCoeff)
{
ExtractObjectAttributes(currentObject);
objetSaillantEnCours = false;
if (currentObject.AngleList.Count > 20)
{
ObjetsSaillantsList.Add(currentObject);
}
}
//Sinon on reste sur le même objet
else
{
if (objetSaillantEnCours)
{
currentObject.AngleList.Add(angleList[i]);
currentObject.DistanceList.Add(distanceList[i]);
}
}
}
//Détection des objets saillants
bool objetFondEnCours = false;
for (int i = 1; i < angleList.Count; i++)
{
//On commence un objet de fond sur un front montant de distance
if (distanceList[i] - distanceList[i - 1] > 0.1 * zoomCoeff)
{
currentObject = new LidarDetectedObject();
currentObject.AngleList.Add(angleList[i]);
currentObject.DistanceList.Add(distanceList[i]);
objetFondEnCours = true;
}
//On termine un objet de fond sur un front descendant de distance
if (distanceList[i] - distanceList[i - 1] < -0.15 * zoomCoeff)
{
ExtractObjectAttributes(currentObject);
objetFondEnCours = false;
if (currentObject.AngleList.Count > 20)
{
ObjetsFondList.Add(currentObject);
}
}
//Sinon on reste sur le même objet
else
{
if (objetFondEnCours)
{
currentObject.AngleList.Add(angleList[i]);
currentObject.DistanceList.Add(distanceList[i]);
}
}
}
foreach (var obj in ObjetsSaillantsList)
{
if (Math.Abs(obj.Largeur - 0.125 * zoomCoeff) < 0.1 * zoomCoeff)
{
ObjetsPoteauPossible.Add(obj);
}
}
List <PolarPointListExtended> objectList = new List <PolarPointListExtended>();
foreach (var obj in ObjetsSaillantsList)
{
PolarPointListExtended currentPolarPointListExtended = new PolarPointListExtended();
currentPolarPointListExtended.polarPointList = new List <PolarPoint>();
//if (obj.Largeur > 0.05 && obj.Largeur < 0.5)
{
for (int i = 0; i < obj.AngleList.Count; i++)
{
currentPolarPointListExtended.polarPointList.Add(new PolarPoint(obj.DistanceList[i], obj.AngleList[i]));
currentPolarPointListExtended.displayColor = System.Drawing.Color.Red;
currentPolarPointListExtended.displayWidth = 8;
}
objectList.Add(currentPolarPointListExtended);
}
}
foreach (var obj in ObjetsPoteauPossible)
{
PolarPointListExtended currentPolarPointListExtended = new PolarPointListExtended();
currentPolarPointListExtended.polarPointList = new List <PolarPoint>();
//if (obj.Largeur > 0.05 && obj.Largeur < 0.5)
{
for (int i = 0; i < obj.AngleList.Count; i++)
{
currentPolarPointListExtended.polarPointList.Add(new PolarPoint(obj.DistanceList[i], obj.AngleList[i]));
currentPolarPointListExtended.displayColor = System.Drawing.Color.Yellow;
currentPolarPointListExtended.displayWidth = 3;
}
objectList.Add(currentPolarPointListExtended);
}
}
foreach (var obj in ObjetsFondList)
{
PolarPointListExtended currentPolarPointListExtended = new PolarPointListExtended();
currentPolarPointListExtended.polarPointList = new List <PolarPoint>();
//if (obj.Largeur > 0.05 && obj.Largeur < 0.5)
{
for (int i = 0; i < obj.AngleList.Count; i++)
{
currentPolarPointListExtended.polarPointList.Add(new PolarPoint(obj.DistanceList[i], obj.AngleList[i]));
currentPolarPointListExtended.displayColor = System.Drawing.Color.Blue;
currentPolarPointListExtended.displayWidth = 6;
}
objectList.Add(currentPolarPointListExtended);
}
}
//OnLidarProcessed(robotId, AngleListProcessed, DistanceListProcessed);
OnLidarProcessed(robotId, angleList, distanceList);
OnLidarObjectProcessed(robotId, objectList);
}
