[STAThread] //à ajouter au projet initial
static void Main(string[] args)
{
SciChartSurface.SetRuntimeLicenseKey(@"<LicenseContract>
<Customer>Universite De Toulon</Customer>
<OrderId>EDUCATIONAL-USE-0128</OrderId>
<LicenseCount>1</LicenseCount>
<IsTrialLicense>false</IsTrialLicense>
<SupportExpires>02/17/2020 00:00:00</SupportExpires>
<ProductCode>SC-WPF-2D-PRO-SITE</ProductCode>
<KeyCode>lwAAAQEAAACS9FAFUqnVAXkAQ3VzdG9tZXI9VW5pdmVyc2l0ZSBEZSBUb3Vsb247T3JkZXJJZD1FRFVDQVRJT05BTC1VU0UtMDEyODtTdWJzY3JpcHRpb25WYWxpZFRvPTE3LUZlYi0yMDIwO1Byb2R1Y3RDb2RlPVNDLVdQRi0yRC1QUk8tU0lURYcbnXYui4rna7TqbkEmUz1V7oD1EwrO3FhU179M9GNhkL/nkD/SUjwJ/46hJZ31CQ==</KeyCode>
</LicenseContract>");
globalWorldMapManagerTeam1 = new GlobalWorldMapManager((int)TeamId.Team1);
//BaseStation RCT
BaseStationUdpMulticastSenderTeam1 = new UDPMulticastSender(0, "224.16.32.79");
BaseStationUdpMulticastReceiverTeam1 = new UDPMulticastReceiver(0, "224.16.32.79");
BaseStationUdpMulticastInterpreterTeam1 = new UDPMulticastInterpreter(0);
StartInterfaces();
refBoxAdapter = new RefereeBoxAdapter.RefereeBoxAdapter();
//Event de réception d'une commande de la réferee box
//refBoxAdapter.OnRefereeBoxCommandEvent += globalWorldMapManagerTeam1.OnRefereeBoxCommandReceived;
//Event de réception de data Multicast sur la base Station Team X
BaseStationUdpMulticastReceiverTeam1.OnDataReceivedEvent += BaseStationUdpMulticastInterpreterTeam1.OnMulticastDataReceived;
//Event d'interprétation d'une localWorldMap à sa réception dans la base station
BaseStationUdpMulticastInterpreterTeam1.OnLocalWorldMapEvent += globalWorldMapManagerTeam1.OnLocalWorldMapReceived;
//Event d'envoi de la global world map sur le Multicast
globalWorldMapManagerTeam1.OnMulticastSendGlobalWorldMapEvent += BaseStationUdpMulticastSenderTeam1.OnMulticastMessageToSendReceived;
lock (ExitLock)
{
// Do whatever setup code you need here
// once we are done wait
Monitor.Wait(ExitLock);
}
}
static void Main(string[] args)
{
// Set this code once in App.xaml.cs or application startup
SciChartSurface.SetRuntimeLicenseKey("RJWA77RbaJDdCRJpg4Iunl5Or6/FPX1xT+Gzu495Eaa0ZahxWi3jkNFDjUb/w70cHXyv7viRTjiNRrYqnqGA+Dc/yzIIzTJlf1s4DJvmQc8TCSrH7MBeQ2ON5lMs/vO0p6rBlkaG+wwnJk7cp4PbOKCfEQ4NsMb8cT9nckfdcWmaKdOQNhHsrw+y1oMR7rIH+rGes0jGGttRDhTOBxwUJK2rBA9Z9PDz2pGOkPjy9fwQ4YY2V4WPeeqM+6eYxnDZ068mnSCPbEnBxpwAldwXTyeWdXv8sn3Dikkwt3yqphQxvs0h6a8Dd6K/9UYni3o8pRkTed6SWodQwICcewfHTyGKQowz3afARj07et2h+becxowq3cRHL+76RyukbIXMfAqLYoT2UzDJNsZqcPPq/kxeXujuhT4SrNF3444MU1GaZZ205KYEMFlz7x/aEnjM6p3BuM6ZuO3Fjf0A0Ki/NBfS6n20E07CTGRtI6AsM2m59orPpI8+24GFlJ9xGTjoRA==");
//On ajoute un gestionnaire d'évènement pour détecter la fermeture de l'application
_handler += new EventHandler(Handler);
SetConsoleCtrlHandler(_handler, true);
//serialPort1 = new ReliableSerialPort(cfgSerialPort.CommName, cfgSerialPort.ComBaudrate, cfgSerialPort.Parity, cfgSerialPort.DataByte, cfgSerialPort.StopByte);
//serialPort1 = new ReliableSerialPort("COM1", 115200, Parity.None, 8, StopBits.One);
int teamId = (int)TeamId.Team1;
int robotId = (int)RobotId.Robot1 + teamId;
usbDriver = new USBVendorNS.USBVendor();
msgDecoder = new MsgDecoder();
msgEncoder = new MsgEncoder();
robotMsgGenerator = new MsgGenerator();
robotMsgProcessor = new MsgProcessor(robotId, competition);
imuProcessor = new ImuProcessor.ImuProcessor(robotId);
lidar_OMD60M_TCP = new LidaRxR2000(50, R2000SamplingRate._72kHz);
perceptionManager = new PerceptionManager(robotId, competition);
kalmanPositioning = new KalmanPositioning.KalmanPositioning(robotId, 50, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.02);
trajectoryPlanner = new TrajectoryGeneratorHolonome(robotId, competition);
localWorldMapManager = new LocalWorldMapManager(robotId, teamId, bypassMulticast: false);
globalWorldMapManager = new GlobalWorldMapManager(robotId, teamId);
switch (competition)
{
case GameMode.RoboCup:
strategyManager = new StrategyRoboCup(robotId, teamId, "224.16.32.79");
break;
case GameMode.Eurobot:
strategyManager = new StrategyEurobot2021(robotId, teamId, "224.16.32.79");
break;
case GameMode.Demo:
break;
}
//strategyManager = new StrategyManagerNS.StrategyManager(robotId, teamId, "224.16.32.79", competition);
robotUdpMulticastSender = new UDPMulticastSender(robotId, "224.16.32.79");
robotUdpMulticastReceiver = new UDPMulticastReceiver(robotId, "224.16.32.79");
robotUdpMulticastInterpreter = new UDPMulticastInterpreter(robotId);
herkulexManager = new HerkulexManager();
herkulexManager.AddServo(ServoId.BrasCentral, HerkulexDescription.JOG_MODE.positionControlJOG);
herkulexManager.AddServo(ServoId.BrasDroit, HerkulexDescription.JOG_MODE.positionControlJOG);
herkulexManager.AddServo(ServoId.BrasGauche, HerkulexDescription.JOG_MODE.positionControlJOG);
herkulexManager.AddServo(ServoId.PorteDrapeau, HerkulexDescription.JOG_MODE.positionControlJOG);
xBoxManette = new XBoxControllerNS.XBoxController(robotId);
//Démarrage des interface de visualisation
if (usingRobotInterface)
{
StartRobotInterface();
}
//if (usingLogReplay)
// StartReplayNavigatorInterface();
//Initialisation du logger
logRecorder = new LogRecorder.LogRecorder();
//Démarrage du log replay si l'interface est utilisée et existe ou si elle n'est pas utilisée, sinon on bloque
logReplay = new LogReplay.LogReplay();
//Liens entre modules
//strategyManager.strategy.OnRefereeBoxCommandEvent += globalWorldMapManager.OnRefereeBoxCommandReceived;
strategyManager.OnGameStateChangedEvent += trajectoryPlanner.OnGameStateChangeReceived;
strategyManager.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived;
//Kalman
perceptionManager.OnAbsolutePositionEvent += kalmanPositioning.OnAbsolutePositionCalculatedEvent;
imuProcessor.OnGyroSpeedEvent += kalmanPositioning.OnGyroRobotSpeedReceived;
robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += kalmanPositioning.OnOdometryRobotSpeedReceived;
kalmanPositioning.OnKalmanLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived;
kalmanPositioning.OnKalmanLocationEvent += perceptionManager.OnPhysicalRobotPositionReceived;
kalmanPositioning.OnKalmanLocationEvent += strategyManager.OnPositionRobotReceived;
//Update des données de la localWorldMap
perceptionManager.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived;
strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived;
strategyManager.OnRoleEvent += localWorldMapManager.OnRoleReceived; //Utile pour l'affichage
strategyManager.OnMessageDisplayEvent += localWorldMapManager.OnMessageDisplayReceived; //Utile pour l'affichage
strategyManager.OnHeatMapStrategyEvent += localWorldMapManager.OnHeatMapStrategyReceived;
strategyManager.OnWaypointEvent += localWorldMapManager.OnWaypointReceived;
strategyManager.OnHeatMapWayPointEvent += localWorldMapManager.OnHeatMapWaypointReceived;
trajectoryPlanner.OnGhostLocationEvent += localWorldMapManager.OnGhostLocationReceived;
//Gestion des events liés à une détection de collision soft
trajectoryPlanner.OnCollisionEvent += kalmanPositioning.OnCollisionReceived;
//trajectoryPlanner.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot;
strategyManager.OnMessageEvent += lidar_OMD60M_TCP.OnMessageReceivedEvent;
strategyManager.On4WheelsPolarSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage4WheelsPolarSpeedPIDSetup;
strategyManager.On4WheelsIndependantSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage4WheelsIndependantSpeedPIDSetup;
strategyManager.On2WheelsPolarSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage2WheelsPolarSpeedPIDSetup;
strategyManager.On2WheelsIndependantSpeedPIDSetupEvent += robotMsgGenerator.GenerateMessage2WheelsIndependantSpeedPIDSetup;
strategyManager.OnSetAsservissementModeEvent += robotMsgGenerator.GenerateMessageSetAsservissementMode;
strategyManager.OnSetSpeedConsigneToMotor += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor;
strategyManager.OnEnableDisableMotorCurrentDataEvent += robotMsgGenerator.GenerateMessageEnableMotorCurrentData;
strategyManager.OnOdometryPointToMeterEvent += robotMsgGenerator.GenerateMessageOdometryPointToMeter;
strategyManager.On4WheelsAngleSetEvent += robotMsgGenerator.GenerateMessage4WheelsAngleSet;
strategyManager.On4WheelsToPolarSetEvent += robotMsgGenerator.GenerateMessage4WheelsToPolarMatrixSet;
strategyManager.On2WheelsAngleSetEvent += robotMsgGenerator.GenerateMessage2WheelsAngleSet;
strategyManager.On2WheelsToPolarSetEvent += robotMsgGenerator.GenerateMessage2WheelsToPolarMatrixSet;
herkulexManager.OnHerkulexSendToSerialEvent += robotMsgGenerator.GenerateMessageForwardHerkulex;
lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += perceptionManager.OnRawLidarDataReceived;
perceptionManager.OnLidarRawDataEvent += localWorldMapManager.OnRawLidarDataReceived;
perceptionManager.OnLidarProcessedDataEvent += localWorldMapManager.OnLidarDataReceived;
perceptionManager.OnLidarProcessedLandmarksEvent += localWorldMapManager.OnLidarDataReceived;
perceptionManager.OnLidarProcessedSegmentsEvent += localWorldMapManager.OnLidarProcessedSegmentsReceived;
//L'envoi des commandes dépend du fait qu'on soit en mode manette ou pas.
//Il faut donc enregistrer les évènement ou pas en fonction de l'activation
//C'est fait plus bas dans le code avec la fonction que l'on appelle
ConfigControlEvents(usingXBoxController);
//Gestion des messages envoyé par le robot
robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot;
//msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage;
msgEncoder.OnMessageEncodedEvent += usbDriver.SendUSBMessage;
//Gestion des messages reçu par le robot
//serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived;
usbDriver.OnUSBDataReceivedEvent += msgDecoder.DecodeMsgReceived;
msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage;
robotMsgProcessor.OnIMURawDataFromRobotGeneratedEvent += imuProcessor.OnIMURawDataReceived;
robotMsgProcessor.OnIOValuesFromRobotGeneratedEvent += strategyManager.OnIOValuesFromRobot;
robotMsgProcessor.OnIOValuesFromRobotGeneratedEvent += perceptionManager.OnIOValuesFromRobotEvent;
landmarksExtractor = new LandmarksExtractor();
landmarksExtractor.OnLinesLandmarksExtractedEvent += perceptionManager.OnLandmarksReceived;
perceptionManager.OnLidarRawDataEvent += landmarksExtractor.OnRobotLidarReceived;
perceptionManager.OnAbsolutePositionEvent += landmarksExtractor.OnRobotPositionReceived;
// robotMsgProcessor.OnMotorsCurrentsFromRobotGeneratedEvent += strategyManager.OnMotorCurrentReceive;
//Le local Manager n'est là que pour assurer le stockage de ma local world map avant affichage et transmission des infos, il ne doit pas calculer quoique ce soit,
//c'est le perception manager qui le fait.
trajectoryPlanner.OnPidSpeedResetEvent += robotMsgGenerator.GenerateMessageResetSpeedPid;
////Event d'interprétation d'une globalWorldMap à sa réception dans le robot
robotUdpMulticastInterpreter.OnRefBoxMessageEvent += strategyManager.OnRefBoxMsgReceived;
/// Event de Transmission des Local World Map du robot vers le multicast
/// Disparaitra quand on voudra jouer sans liaison multicast
localWorldMapManager.OnMulticastSendLocalWorldMapEvent += robotUdpMulticastSender.OnMulticastMessageToSendReceived;
/// Events de réception des localWorldMap
/// Soit en direct si on se transmet à nous même, soit via le Multicast pour transmettre aux autres robots
localWorldMapManager.OnLocalWorldMapToGlobalWorldMapGeneratorEvent += globalWorldMapManager.OnLocalWorldMapReceived;
robotUdpMulticastInterpreter.OnLocalWorldMapEvent += globalWorldMapManager.OnLocalWorldMapReceived;
/// Event généré lorsque la Global World Map a été calculée.
/// Elle n'a pas vocation à être renvoyée à tous les robots puisqu'on la génère dans chaque robot en parallèle
globalWorldMapManager.OnGlobalWorldMapEvent += strategyManager.OnGlobalWorldMapReceived;
/// Event de Réception de data Multicast sur le robot
robotUdpMulticastReceiver.OnDataReceivedEvent += robotUdpMulticastInterpreter.OnMulticastDataReceived;
/// LOGGER related events
perceptionManager.OnLidarRawDataEvent += logRecorder.OnRawLidarDataReceived;
robotMsgProcessor.OnIMURawDataFromRobotGeneratedEvent += logRecorder.OnIMURawDataReceived;
robotMsgProcessor.OnPolarOdometrySpeedFromRobotEvent += logRecorder.OnPolarSpeedDataReceived;
//omniCamera.OpenCvMatImageEvent += logRecorder.OnOpenCVMatImageReceived;
//strategyManagerDictionary.Add(robotId, strategyManager);
trajectoryPlanner.InitRobotPosition(0, 0, 0);
strategyManager.InitStrategy(robotId, teamId);
while (!exitSystem)
{
Thread.Sleep(500);
}
}
[STAThread] //à ajouter au projet initial
static void Main(string[] args)
{
// Set this code once in App.xaml.cs or application startup
SciChartSurface.SetRuntimeLicenseKey("wsCOsvBlAs2dax4o8qBefxMi4Qe5BVWax7TGOMLcwzWFYRNCa/f1rA5VA1ITvLHSULvhDMKVTc+niao6URAUXmGZ9W8jv/4jtziBzFZ6Z15ek6SLU49eIqJxGoQEFWvjANJqzp0asw+zvLV0HMirjannvDRj4i/WoELfYDubEGO1O+oAToiJlgD/e2lVqg3F8JREvC0iqBbNrmfeUCQdhHt6SKS2QpdmOoGbvtCossAezGNxv92oUbog6YIhtpSyGikCEwwKSDrlKlAab6302LLyFsITqogZychLYrVXJTFvFVnDfnkQ9cDi7017vT5flesZwIzeH497lzGp3B8fKWFQyZemD2RzlQkvj5GUWBwxiKAHrYMnQjJ/PsfojF1idPEEconVsh1LoYofNk2v/Up8AzXEAvxWUEcgzANeQggaUNy+OFet8b/yACa/bgYG7QYzFQZzgdng8IK4vCPdtg4/x7g5EdovN2PI9vB76coMuKnNVPnZN60kSjtd/24N8A==");
//waypointGeneratorList = new List<WaypointGenerator>();
trajectoryPlannerList = new List <TrajectoryPlanner>();
sensorSimulatorList = new List <SensorSimulator.SensorSimulator>();
kalmanPositioningList = new List <KalmanPositioning.KalmanPositioning>();
strategyManagerDictionary = new Dictionary <int, StrategyManagerNS.StrategyManager>();
localWorldMapManagerList = new List <LocalWorldMapManager>();
perceptionManagerList = new List <PerceptionManager>();
robotUdpMulticastSenderList = new List <UDPMulticastSender>();
robotUdpMulticastReceiverList = new List <UDPMulticastReceiver>();
robotUdpMulticastInterpreterList = new List <UDPMulticastInterpreter>();
physicalSimulator = new PhysicalSimulator.PhysicalSimulator("RoboCup");
globalWorldMapManagerTeam1 = new GlobalWorldMapManager((int)TeamId.Team1);
globalWorldMapManagerTeam2 = new GlobalWorldMapManager((int)TeamId.Team2);
//BaseStation RCT
BaseStationUdpMulticastSenderTeam1 = new UDPMulticastSender(0, "224.16.32.79");
BaseStationUdpMulticastReceiverTeam1 = new UDPMulticastReceiver(0, "224.16.32.79");
BaseStationUdpMulticastInterpreterTeam1 = new UDPMulticastInterpreter(0);
//BaseStation TuE
BaseStationUdpMulticastSenderTeam2 = new UDPMulticastSender(0, "224.16.32.63");
BaseStationUdpMulticastReceiverTeam2 = new UDPMulticastReceiver(0, "224.16.32.63");
BaseStationUdpMulticastInterpreterTeam2 = new UDPMulticastInterpreter(0);
for (int i = 0; i < nbPlayersTeam1; i++)
{
CreatePlayer((int)TeamId.Team1, i, team1PlayerNames[i], "224.16.32.79");
}
for (int i = 0; i < nbPlayersTeam2; i++)
{
CreatePlayer((int)TeamId.Team2, i, team2PlayerNames[i], "224.16.32.63");
}
//DefineRoles();
StartInterfaces();
refBoxAdapter = new RefereeBoxAdapter.RefereeBoxAdapter();
//Event de réception d'une commande de la réferee box
refBoxAdapter.OnMulticastSendRefBoxCommandEvent += BaseStationUdpMulticastSenderTeam1.OnMulticastMessageToSendReceived;
refBoxAdapter.OnMulticastSendRefBoxCommandEvent += BaseStationUdpMulticastSenderTeam2.OnMulticastMessageToSendReceived;
//Event de réception de data Multicast sur la base Station Team X
BaseStationUdpMulticastReceiverTeam1.OnDataReceivedEvent += BaseStationUdpMulticastInterpreterTeam1.OnMulticastDataReceived;
BaseStationUdpMulticastReceiverTeam2.OnDataReceivedEvent += BaseStationUdpMulticastInterpreterTeam2.OnMulticastDataReceived;
//Event d'interprétation d'une localWorldMap à sa réception dans la base station
BaseStationUdpMulticastInterpreterTeam1.OnLocalWorldMapEvent += globalWorldMapManagerTeam1.OnLocalWorldMapReceived;
BaseStationUdpMulticastInterpreterTeam2.OnLocalWorldMapEvent += globalWorldMapManagerTeam2.OnLocalWorldMapReceived;
//Event d'envoi de la global world map sur le Multicast
globalWorldMapManagerTeam1.OnMulticastSendGlobalWorldMapEvent += BaseStationUdpMulticastSenderTeam1.OnMulticastMessageToSendReceived;
globalWorldMapManagerTeam2.OnMulticastSendGlobalWorldMapEvent += BaseStationUdpMulticastSenderTeam2.OnMulticastMessageToSendReceived;
lock (ExitLock)
{
// Do whatever setup code you need here
// once we are done wait
Monitor.Wait(ExitLock);
}
}
private static void CreatePlayer(int TeamNumber, int RobotNumber, string Name, string multicastIpAddress)
{
int robotId = TeamNumber + RobotNumber;
var strategyManager = new StrategyManagerNS.StrategyManager(robotId, TeamNumber, multicastIpAddress, GameMode.RoboCup);
//var waypointGenerator = new WaypointGenerator(robotId, GameMode.RoboCup);
var trajectoryPlanner = new TrajectoryPlanner(robotId, GameMode.RoboCup);
var sensorSimulator = new SensorSimulator.SensorSimulator(robotId);
var kalmanPositioning = new KalmanPositioning.KalmanPositioning(robotId, 50, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.02);
var localWorldMapManager = new LocalWorldMapManager(robotId, TeamNumber, bypassMulticast: false);
//var lidarSimulator = new LidarSimulator.LidarSimulator(robotId);
var perceptionSimulator = new PerceptionManager(robotId, GameMode.RoboCup);
UDPMulticastSender robotUdpMulticastSender = null;
UDPMulticastReceiver robotUdpMulticastReceiver = null;
UDPMulticastInterpreter robotUdpMulticastInterpreter = null;
if (TeamNumber == (int)TeamId.Team1)
{
robotUdpMulticastSender = new UDPMulticastSender(robotId, "224.16.32.79");
robotUdpMulticastReceiver = new UDPMulticastReceiver(robotId, "224.16.32.79");
robotUdpMulticastInterpreter = new UDPMulticastInterpreter(robotId);
}
else if (TeamNumber == (int)TeamId.Team2)
{
robotUdpMulticastSender = new UDPMulticastSender(robotId, "224.16.32.63");
robotUdpMulticastReceiver = new UDPMulticastReceiver(robotId, "224.16.32.63");
robotUdpMulticastInterpreter = new UDPMulticastInterpreter(robotId);
}
//Liens entre modules
strategyManager.strategy.OnGameStateChangedEvent += trajectoryPlanner.OnGameStateChangeReceived;
strategyManager.strategy.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived;
((StrategyRoboCup)strategyManager.strategy).OnShootRequestEvent += physicalSimulator.OnShootOrderReceived;
trajectoryPlanner.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed;
//Gestion des events liés à une détection de collision soft
trajectoryPlanner.OnCollisionEvent += kalmanPositioning.OnCollisionReceived;
trajectoryPlanner.OnCollisionEvent += physicalSimulator.OnCollisionReceived;
////physicalSimulator.OnPhysicalRobotLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived; //replacé par les 5 lignes suivantes
physicalSimulator.OnPhysicalRobotLocationEvent += sensorSimulator.OnPhysicalRobotPositionReceived;
physicalSimulator.OnPhysicalBallHandlingEvent += sensorSimulator.OnPhysicalBallHandlingReceived;
sensorSimulator.OnCamLidarSimulatedRobotPositionEvent += kalmanPositioning.OnCamLidarSimulatedRobotPositionReceived;
sensorSimulator.OnGyroSimulatedRobotSpeedEvent += kalmanPositioning.OnGyroRobotSpeedReceived;
sensorSimulator.OnOdometrySimulatedRobotSpeedEvent += kalmanPositioning.OnOdometryRobotSpeedReceived;
sensorSimulator.OnBallHandlingSimulatedEvent += ((StrategyRoboCup)strategyManager.strategy).OnBallHandlingSensorInfoReceived;
kalmanPositioning.OnKalmanLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived;
//physicalSimulator.OnPhysicalRobotLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived; //ajout
kalmanPositioning.OnKalmanLocationEvent += perceptionSimulator.OnPhysicalRobotPositionReceived;
kalmanPositioning.OnKalmanLocationEvent += strategyManager.strategy.OnPositionRobotReceived;
//physicalSimulator.OnPhysicalRobotLocationEvent += perceptionSimulator.OnPhysicalRobotPositionReceived; //ajout
physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived;
physicalSimulator.OnPhysicalBallPositionListEvent += perceptionSimulator.OnPhysicalBallPositionListReceived;
//Update des données de la localWorldMap
perceptionSimulator.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived;
strategyManager.strategy.OnDestinationEvent += localWorldMapManager.OnDestinationReceived;
strategyManager.strategy.OnRoleEvent += localWorldMapManager.OnRoleReceived; //Utile pour l'affichage
strategyManager.strategy.OnBallHandlingStateEvent += localWorldMapManager.OnBallHandlingStateReceived;
strategyManager.strategy.OnMessageDisplayEvent += localWorldMapManager.OnMessageDisplayReceived; //Utile pour l'affichage
//strategyManager.strategy.OnPlayingSideEvent += localWorldMapManager.OnPlayingSideReceived; //inutile
strategyManager.strategy.OnHeatMapStrategyEvent += localWorldMapManager.OnHeatMapStrategyReceived;
strategyManager.strategy.OnWaypointEvent += localWorldMapManager.OnWaypointReceived;
strategyManager.strategy.OnHeatMapWayPointEvent += localWorldMapManager.OnHeatMapWaypointReceived;
//waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived;
//waypointGenerator.OnHeatMapEvent += localWorldMapManager.OnHeatMapWaypointReceived;
trajectoryPlanner.OnGhostLocationEvent += localWorldMapManager.OnGhostLocationReceived;
//Event de Réception de data Multicast sur le robot
robotUdpMulticastReceiver.OnDataReceivedEvent += robotUdpMulticastInterpreter.OnMulticastDataReceived;
//Event d'interprétation d'une globalWorldMap à sa réception dans le robot
robotUdpMulticastInterpreter.OnGlobalWorldMapEvent += strategyManager.strategy.OnGlobalWorldMapReceived;
robotUdpMulticastInterpreter.OnRefBoxMessageEvent += strategyManager.strategy.OnRefBoxMsgReceived;
//robotUdpMulticastInterpreter.OnGlobalWorldMapEvent += waypointGenerator.OnGlobalWorldMapReceived;
robotUdpMulticastInterpreter.OnGlobalWorldMapEvent += perceptionSimulator.OnGlobalWorldMapReceived;
//Event de Transmission des Local World Map du robot vers le multicast
localWorldMapManager.OnMulticastSendLocalWorldMapEvent += robotUdpMulticastSender.OnMulticastMessageToSendReceived;
strategyManagerDictionary.Add(robotId, strategyManager);
//waypointGeneratorList.Add(waypointGenerator);
trajectoryPlannerList.Add(trajectoryPlanner);
sensorSimulatorList.Add(sensorSimulator);
kalmanPositioningList.Add(kalmanPositioning);
localWorldMapManagerList.Add(localWorldMapManager);
//lidarSimulatorList.Add(lidarSimulator);
perceptionManagerList.Add(perceptionSimulator);
robotUdpMulticastReceiverList.Add(robotUdpMulticastReceiver);
robotUdpMulticastSenderList.Add(robotUdpMulticastSender);
robotUdpMulticastInterpreterList.Add(robotUdpMulticastInterpreter);
double xInit, yInit, thetaInit;
if (TeamNumber == (int)TeamId.Team1)
{
xInit = 2 * RobotNumber + 2;
yInit = -6.5;
thetaInit = Math.PI / 2;
}
else
{
xInit = -(2 * RobotNumber + 2);
yInit = +6.5;
thetaInit = 0;
}
physicalSimulator.RegisterRobot(robotId, xInit, yInit);
trajectoryPlanner.InitRobotPosition(xInit, yInit, thetaInit);
}
