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)); ////} } } }