public void OnLidarObjectsReceived(object sender, EventArgsLibrary.PolarPointListExtendedListArgs e)
{
if (localWorldMap == null || localWorldMap.robotLocation == null)
{
return;
}
if (RobotId == e.RobotId)
{
localWorldMap.lidarObjectList = e.ObjectList;
}
}
public void OnLidarObjectsReceived(object sender, EventArgsLibrary.PolarPointListExtendedListArgs e)
{
if (robotPerception.robotKalmanLocation != null)
{
lock (physicalObjectList)
{
physicalObjectList.Clear();
double xRobot = robotPerception.robotKalmanLocation.X;
double yRobot = robotPerception.robotKalmanLocation.Y;
double angleRobot = robotPerception.robotKalmanLocation.Theta;
//On récupère la liste des objets physiques vus par le robot (y compris lui-même en simulation)
foreach (var obj in e.ObjectList)
{
double angle = obj.polarPointList[0].Angle;
double distance = obj.polarPointList[0].Distance;
double xObjetRefTerrain = xRobot + distance * Math.Cos(angle + angleRobot);
double yObjetRefTerrain = yRobot + distance * Math.Sin(angle + angleRobot);
//Code spécifique Eurobot
//On s'occupe des obstacles dans le terrain qui sont a priori des robots
//if (Math.Abs(xObjetRefTerrain) < 1.5 && Math.Abs(yObjetRefTerrain) < 1.0)
{
double rayon = 0.2;
if (distance > 0.05) //On exclut les obstacles trop proches
{
physicalObjectList.Add(new LocationExtended(xObjetRefTerrain, yObjetRefTerrain, 0, 0, 0, 0, ObjectType.Robot));
////On génère une liste de points périmètres des obstacle pour les interdire
//for (double anglePourtour = 0; anglePourtour < 2 * Math.PI; anglePourtour += 2 * Math.PI / 5)
//{
// physicalObjectList.Add(new LocationExtended(xObjetRefTerrain + rayon * Math.Cos(anglePourtour), yObjetRefTerrain + rayon * Math.Sin(anglePourtour), 0, 0, 0, 0, ObjectType.Robot));
//}
}
}
}
}
}
}
public void OnLidarObjectsReceived(object sender, EventArgsLibrary.PolarPointListExtendedListArgs e)
{
if (robotPerception.robotKalmanLocation != null)
{
lock (physicalObjectList)
{
physicalObjectList.Clear();
double xRobot = robotPerception.robotKalmanLocation.X;
double yRobot = robotPerception.robotKalmanLocation.Y;
double angleRobot = robotPerception.robotKalmanLocation.Theta;
//On récupère la liste des objets physiques vus par le robot (y compris lui-même en simulation)
foreach (var obj in e.ObjectList)
{
double angle = obj.polarPointList[0].Angle;
double distance = obj.polarPointList[0].Distance;
double xObjetRefTerrain = xRobot + distance * Math.Cos(angle + angleRobot);
double yObjetRefTerrain = yRobot + distance * Math.Sin(angle + angleRobot);
//Code spécifique Eurobot
//On s'occupe des obstacles dans le terrain qui sont a priori des robots
if (Math.Abs(xObjetRefTerrain) < 1.5 && Math.Abs(yObjetRefTerrain) < 1.0)
{
double rayon = 0.2;
if (distance > 0.05) //On exclut les obstacles trop proches
{
physicalObjectList.Add(new LocationExtended(xObjetRefTerrain, yObjetRefTerrain, 0, 0, 0, 0, ObjectType.Robot));
////On génère une liste de points périmètres des obstacle pour les interdire
//for (double anglePourtour = 0; anglePourtour < 2 * Math.PI; anglePourtour += 2 * Math.PI / 5)
//{
// physicalObjectList.Add(new LocationExtended(xObjetRefTerrain + rayon * Math.Cos(anglePourtour), yObjetRefTerrain + rayon * Math.Sin(anglePourtour), 0, 0, 0, 0, ObjectType.Robot));
//}
}
}
}
//double borderAvoidanceZone = 0.05;
////On rajoute les bordures du terrain à la main :
//physicalObjectList.Add(new LocationExtended(0, -1+ borderAvoidanceZone, 0, 0, 0, 0, ObjectType.LimiteHorizontaleBasse));
//physicalObjectList.Add(new LocationExtended(0, 1- borderAvoidanceZone, 0, 0, 0, 0, ObjectType.LimiteHorizontaleHaute));
//physicalObjectList.Add(new LocationExtended(-1.5+ borderAvoidanceZone, 0, 0, 0, 0, 0, ObjectType.LimiteVerticaleGauche));
//physicalObjectList.Add(new LocationExtended(1.5- borderAvoidanceZone, 0, 0, 0, 0, 0, ObjectType.LimiteVerticaleDroite));
//if (playingTeam == Equipe.Jaune)
//{
// physicalObjectList.Add(new LocationExtended(1.05, 0.08, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(1.05, -0.49, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(1.3, 0.08, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(1.3, -0.49, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(1.05, (0.08 - 0.49) / 2.0, 0, 0, 0, 0, ObjectType.Obstacle));
//}
//else if (playingTeam == Equipe.Bleue)
//{
// physicalObjectList.Add(new LocationExtended(-1.05, 0.08, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(-1.3, 0.08, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(-1.05, -0.49, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(-1.3, -0.49, 0, 0, 0, 0, ObjectType.Obstacle));
// physicalObjectList.Add(new LocationExtended(-1.05, (0.08-0.49)/2.0, 0, 0, 0, 0, ObjectType.Obstacle));
//}
//physicalObjectList.Add(new LocationExtended(0, 0.7, 0, 0, 0, 0, ObjectType.Obstacle));
//physicalObjectList.Add(new LocationExtended(-0.611, 0.85, 0, 0, 0, 0, ObjectType.Obstacle));
//physicalObjectList.Add(new LocationExtended(0.611, 0.85, 0, 0, 0, 0, ObjectType.Obstacle));
////for (double x = -1.5; x <= 1.5; x += 0.35)
////{
//// physicalObjectList.Add(new LocationExtended(x, -1, 0, 0, 0, 0, ObjectType.Obstacle));
//// physicalObjectList.Add(new LocationExtended(x, 1, 0, 0, 0, 0, ObjectType.Obstacle));
////}
////for (double y = -0.8; y <= 0.8; y += 0.35)
////{
//// physicalObjectList.Add(new LocationExtended(-1.5, y, 0, 0, 0, 0, ObjectType.Obstacle));
//// physicalObjectList.Add(new LocationExtended(1.5, y, 0, 0, 0, 0, ObjectType.Obstacle));
////}
}
}
}
