public PhysicalSimulator(string typeTerrain) { switch (typeTerrain) { case "Cachan": LengthAireDeJeu = 8; WidthAireDeJeu = 4; break; case "RoboCup": LengthAireDeJeu = 24; WidthAireDeJeu = 16; break; default: break; } ballSimulatedList.TryAdd(0, new PhysicalBallSimulator(0, 0)); //ballSimulatedList.Add(1, new PhysicalBallSimulator(3, 0)); //ballSimulatedList.Add(2, new PhysicalBallSimulator(6, 0)); highFrequencyTimer = new HighFreqTimer(fSampling); highFrequencyTimer.Tick += HighFrequencyTimer_Tick; highFrequencyTimer.Start(); }
public GlobalWorldMapManager(int teamId) { TeamId = teamId; globalWorldMapSendTimer = new HighFreqTimer(freqRafraichissementWorldMap); globalWorldMapSendTimer.Tick += GlobalWorldMapSendTimer_Tick; globalWorldMapSendTimer.Start(); }
public LidarSimulator(int id) { robotId = id; //Timer timerSensor = new HighFreqTimer(50); timerSensor.Tick += TimerSensor_Tick;; timerSensor.Start(); }
[STAThread] //à ajouter au projet initial static void Main(string[] args) { SciChartSurface.SetRuntimeLicenseKey(@"<LicenseContract> <Customer>University of Toulon</Customer> <OrderId>EDUCATIONAL-USE-0109</OrderId> <LicenseCount>1</LicenseCount> <IsTrialLicense>false</IsTrialLicense> <SupportExpires>11/04/2019 00:00:00</SupportExpires> <ProductCode>SC-WPF-SDK-PRO-SITE</ProductCode> <KeyCode>lwABAQEAAABZVzOfQ0zVAQEAewBDdXN0b21lcj1Vbml2ZXJzaXR5IG9mICBUb3Vsb247T3JkZXJJZD1FRFVDQVRJT05BTC1VU0UtMDEwOTtTdWJzY3JpcHRpb25WYWxpZFRvPTA0LU5vdi0yMDE5O1Byb2R1Y3RDb2RlPVNDLVdQRi1TREstUFJPLVNJVEWDf0QgB8GnCQXI6yAqNM2njjnGbUt2KsujTDzeE+k69K1XYVF1s1x1Hb/i/E3GHaU=</KeyCode> </LicenseContract>"); switch (robotMode) { case RobotMode.Standard: usingLidar = true; usingLogging = false; usingLogReplay = false; break; case RobotMode.LidarAcquisition: usingLidar = true; usingLogging = true; usingLogReplay = false; break; case RobotMode.LidarReplay: usingLidar = false; usingLogging = false; usingLogReplay = true; break; case RobotMode.Nolidar: usingLidar = false; usingLogging = false; usingLogReplay = false; break; } ethernetTeamNetworkAdapter = new EthernetTeamNetworkAdapter(); serialPort1 = new ReliableSerialPort("COM1", 115200, Parity.None, 8, StopBits.One); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); robotMsgGenerator = new RobotMsgGenerator(); robotMsgProcessor = new RobotMsgProcessor(); physicalSimulator = new PhysicalSimulator.PhysicalSimulator(); int robotId = (int)TeamId.Team1 + (int)RobotId.Robot1; int teamId = (int)TeamId.Team1; physicalSimulator.RegisterRobot(robotId, 0, 0); robotPilot = new RobotPilot.RobotPilot(robotId); refBoxAdapter = new RefereeBoxAdapter.RefereeBoxAdapter(); trajectoryPlanner = new TrajectoryPlanner(robotId); waypointGenerator = new WaypointGenerator(robotId); strategyManager = new StrategyManager.StrategyManager(robotId); localWorldMapManager = new LocalWorldMapManager(robotId, teamId); lidarSimulator = new LidarSimulator.LidarSimulator(robotId); perceptionSimulator = new PerceptionSimulator(robotId); if (usingLidar) { lidar_OMD60M = new Lidar_OMD60M(robotId); lidarProcessor = new LidarProcessor.LidarProcessor(robotId); } xBoxManette = new XBoxController.XBoxController(robotId); if (!usingSimulatedCamera) { omniCamera = new BaslerCameraAdapter(); } else { omniCameraSimulator = new SimulatedCamera.SimulatedCamera(); } imageProcessingPositionFromOmniCamera = new ImageProcessingPositionFromOmniCamera(); //Démarrage des interface de visualisation StartInterfaces(); //Démarrage du logging if (usingLogging) { logRecorder = new LogRecorder.LogRecorder(); } if (usingLogReplay) { logReplay = new LogReplay.LogReplay(); lidarProcessor = new LidarProcessor.LidarProcessor(robotId); } //Liens entre modules strategyManager.OnDestinationEvent += waypointGenerator.OnDestinationReceived; strategyManager.OnHeatMapEvent += waypointGenerator.OnStrategyHeatMapReceived; waypointGenerator.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived; if (!usingXBoxController) { trajectoryPlanner.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; robotPilot.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; } else { //Sur evenement xx -->> Action a effectuer xBoxManette.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; xBoxManette.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; xBoxManette.OnPriseBalleEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor; xBoxManette.OnMoveTirUpEvent += robotMsgGenerator.GenerateMessageMoveTirUp; xBoxManette.OnMoveTirDownEvent += robotMsgGenerator.GenerateMessageMoveTirDown; xBoxManette.OnTirEvent += robotMsgGenerator.GenerateMessageTir; } robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; physicalSimulator.OnPhysicalRobotPositionEvent += trajectoryPlanner.OnPhysicalPositionReceived; physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived; physicalSimulator.OnPhysicalRobotPositionEvent += perceptionSimulator.OnPhysicalRobotPositionReceived; physicalSimulator.OnPhysicalBallPositionEvent += perceptionSimulator.OnPhysicalBallPositionReceived; perceptionSimulator.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived; //lidarSimulator.OnSimulatedLidarEvent += localWorldMapManager.OnRawLidarDataReceived; strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived; waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived; strategyManager.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; //waypointGenerator.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; if (usingLidar) { lidar_OMD60M.OnLidarEvent += lidarProcessor.OnRawLidarDataReceived; lidarProcessor.OnLidarProcessedEvent += localWorldMapManager.OnRawLidarDataReceived; } //Event de recording if (usingLogging) { lidar_OMD60M.OnLidarEvent += logRecorder.OnRawLidarDataReceived; } //Event de replay if (usingLogReplay) { logReplay.OnLidarEvent += lidarProcessor.OnRawLidarDataReceived; lidarProcessor.OnLidarProcessedEvent += localWorldMapManager.OnRawLidarDataReceived; lidarProcessor.OnLidarObjectProcessedEvent += localWorldMapManager.OnLidarObjectsReceived; } //Timer de stratégie timerStrategie = new HighFreqTimer(0.5); timerStrategie.Tick += TimerStrategie_Tick; timerStrategie.Start(); lock (ExitLock) { // Do whatever setup code you need here // once we are done wait Monitor.Wait(ExitLock); } }
[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>"); //TODO : Créer un projet World... ethernetTeamNetworkAdapter = new EthernetTeamNetworkAdapter(); serialPort1 = new ReliableSerialPort("FTDI", 230400 /*115200*/, Parity.None, 8, StopBits.One); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); robotMsgGenerator = new RobotMsgGenerator(); robotMsgProcessor = new RobotMsgProcessor(); physicalSimulator = new PhysicalSimulator.PhysicalSimulator(); int robotId = (int)TeamId.Team1 + (int)RobotId.Robot1; int teamId = (int)TeamId.Team1; physicalSimulator.RegisterRobot(robotId, 0, 0); robotPilot = new RobotPilot.RobotPilot(robotId); refBoxAdapter = new RefereeBoxAdapter.RefereeBoxAdapter(); trajectoryPlanner = new TrajectoryPlanner(robotId); waypointGenerator = new WaypointGenerator(robotId); strategyManager = new StrategyManager.StrategyManager(robotId); localWorldMapManager = new LocalWorldMapManager(robotId, teamId); lidarSimulator = new LidarSimulator.LidarSimulator(robotId); perceptionSimulator = new PerceptionSimulator(robotId); if (usingLidar) { lidar_OMD60M = new Lidar_OMD60M(robotId); } xBoxManette = new XBoxController.XBoxController(robotId); if (!usingSimulatedCamera) { omniCamera = new BaslerCameraAdapter(); } else { omniCameraSimulator = new SimulatedCamera.SimulatedCamera(); } imageProcessingPositionFromOmniCamera = new ImageProcessingPositionFromOmniCamera(); StartInterfaces(); //Liens entre modules strategyManager.OnDestinationEvent += waypointGenerator.OnDestinationReceived; strategyManager.OnHeatMapEvent += waypointGenerator.OnStrategyHeatMapReceived; waypointGenerator.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived; if (!usingXBoxController) { trajectoryPlanner.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; robotPilot.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; } else { //Sur evenement xx -->> Action a effectuer xBoxManette.OnSpeedConsigneEvent += physicalSimulator.SetRobotSpeed; xBoxManette.OnSpeedConsigneEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToRobot; xBoxManette.OnPriseBalleEvent += robotMsgGenerator.GenerateMessageSetSpeedConsigneToMotor; xBoxManette.OnMoveTirUpEvent += robotMsgGenerator.GenerateMessageMoveTirUp; xBoxManette.OnMoveTirDownEvent += robotMsgGenerator.GenerateMessageMoveTirDown; xBoxManette.OnTirEvent += robotMsgGenerator.GenerateMessageTir; } robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; physicalSimulator.OnPhysicalRobotPositionEvent += trajectoryPlanner.OnPhysicalPositionReceived; physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived; physicalSimulator.OnPhysicalRobotPositionEvent += perceptionSimulator.OnPhysicalRobotPositionReceived; physicalSimulator.OnPhysicalBallPositionEvent += perceptionSimulator.OnPhysicalBallPositionReceived; perceptionSimulator.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived; //lidarSimulator.OnSimulatedLidarEvent += localWorldMapManager.OnRawLidarDataReceived; strategyManager.OnDestinationEvent += localWorldMapManager.OnDestinationReceived; waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived; strategyManager.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; //waypointGenerator.OnHeatMapEvent += localWorldMapManager.OnHeatMapReceived; if (usingLidar) { lidar_OMD60M.OnLidarEvent += localWorldMapManager.OnRawLidarDataReceived; } //Timer de stratégie timerStrategie = new HighFreqTimer(0.5); timerStrategie.Tick += TimerStrategie_Tick; timerStrategie.Start(); lock (ExitLock) { // Do whatever setup code you need here // once we are done wait Monitor.Wait(ExitLock); } }
static void Main(string[] args) { SetConsoleCtrlHandler(new HandlerRoutine(ConsoleCtrlCheck), true); // 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=="); switch (robotMode) { case RobotMode.Standard: usingLidar = true; usingCamera = true; usingLogging = false; usingLogReplay = false; break; case RobotMode.Acquisition: usingLidar = true; usingCamera = true; usingLogging = true; usingLogReplay = false; break; case RobotMode.Replay: usingLidar = false; usingCamera = false; usingLogging = false; usingLogReplay = true; break; case RobotMode.Nolidar: usingLidar = false; usingCamera = true; usingLogging = false; usingLogReplay = false; break; case RobotMode.NoCamera: usingLidar = true; usingCamera = false; usingLogging = false; usingLogReplay = false; break; } int robotId = (int)TeamId.Team1 + (int)RobotId.Robot1; int teamId = (int)TeamId.Team1; serialPort1 = new ReliableSerialPort("COM1", 115200, Parity.None, 8, StopBits.One); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); robotMsgGenerator = new MsgGenerator(); robotMsgProcessor = new MsgProcessor(robotId, GameMode.RoboCup); robotPilot = new RobotPilot.RobotPilot(robotId); strategyManager = new StrategyManager(robotId, teamId, "224.16.32.79", GameMode.RoboCup); waypointGenerator = new WaypointGenerator(robotId, GameMode.RoboCup); trajectoryPlanner = new TrajectoryPlanner(robotId, GameMode.RoboCup); kalmanPositioning = new KalmanPositioning.KalmanPositioning(robotId, 50, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.02); localWorldMapManager = new LocalWorldMapManager(robotId, teamId, bypassMulticast: false); //lidarSimulator = new LidarSimulator.LidarSimulator(robotId); perceptionManager = new PerceptionManager(robotId, GameMode.RoboCup); imuProcessor = new ImuProcessor.ImuProcessor(robotId); if (usingYolo) { yoloDetector = new YoloObjectDetector.YoloObjectDetector(false); //Instancie un detecteur avec un Wrappeur Yolo utilisant le GPU } if (usingLidar) { lidar_OMD60M_TCP = new LidaRxR2000(); } if (usingLidar || usingLogReplay) { lidarProcessor = new LidarProcessor.LidarProcessor(robotId, GameMode.RoboCup); } xBoxManette = new XBoxController.XBoxController(robotId); if (usingCamera || usingLogReplay) { imageProcessingPositionFromOmniCamera = new ImageProcessingPositionFromOmniCamera(); absolutePositionEstimator = new AbsolutePositionEstimator(robotId); } if (usingCamera) { omniCamera = new BaslerCameraAdapter(); omniCamera.CameraInit(); //omniCamera.BitmapPanoramaImageEvent += absolutePositionEstimator.AbsolutePositionEvaluation; } if (usingImageExtractor && usingCamera) { imgSaver = new ImageSaver.ImageSaver(); omniCamera.BitmapPanoramaImageEvent += imgSaver.OnSaveBitmapImage; } //Démarrage des interface de visualisation if (usingRobotInterface) { StartRobotInterface(); } if (usingCameraInterface) { StartCameraInterface(); } //if (usingLogReplay) // StartReplayNavigatorInterface(); //Démarrage du logger si besoin if (usingLogging) { 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 if (usingLogReplay) { logReplay = new LogReplay.LogReplay(); } //Liens entre modules strategyManager.strategy.OnDestinationEvent += waypointGenerator.OnDestinationReceived; strategyManager.strategy.OnHeatMapStrategyEvent += waypointGenerator.OnStrategyHeatMapReceived; waypointGenerator.OnWaypointEvent += trajectoryPlanner.OnWaypointReceived; //Filtre de Kalman perceptionManager.OnAbsolutePositionEvent += kalmanPositioning.OnAbsolutePositionCalculatedEvent; robotMsgProcessor.OnSpeedPolarOdometryFromRobotEvent += kalmanPositioning.OnOdometryRobotSpeedReceived; imuProcessor.OnGyroSpeedEvent += kalmanPositioning.OnGyroRobotSpeedReceived; kalmanPositioning.OnKalmanLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived; kalmanPositioning.OnKalmanLocationEvent += perceptionManager.OnPhysicalRobotPositionReceived; //kalmanPositioning.OnKalmanLocationEvent += strategyManager.OnPositionRobotReceived; //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(useXBoxController: true); //Gestion des messages envoyé par le robot robotMsgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeMessageToRobot; msgEncoder.OnMessageEncodedEvent += serialPort1.SendMessage; //Gestion des messages reçu par le robot serialPort1.OnDataReceivedEvent += msgDecoder.DecodeMsgReceived; msgDecoder.OnMessageDecodedEvent += robotMsgProcessor.ProcessRobotDecodedMessage; robotMsgProcessor.OnIMURawDataFromRobotGeneratedEvent += imuProcessor.OnIMURawDataReceived; //physicalSimulator.OnPhysicalRobotLocationEvent += trajectoryPlanner.OnPhysicalPositionReceived; //physicalSimulator.OnPhysicicalObjectListLocationEvent += perceptionSimulator.OnPhysicalObjectListLocationReceived; //physicalSimulator.OnPhysicalRobotLocationEvent += perceptionSimulator.OnPhysicalRobotPositionReceived; //physicalSimulator.OnPhysicalBallPositionEvent += perceptionSimulator.OnPhysicalBallPositionReceived; perceptionManager.OnPerceptionEvent += localWorldMapManager.OnPerceptionReceived; strategyManager.strategy.OnDestinationEvent += localWorldMapManager.OnDestinationReceived; waypointGenerator.OnWaypointEvent += localWorldMapManager.OnWaypointReceived; strategyManager.strategy.OnHeatMapStrategyEvent += localWorldMapManager.OnHeatMapStrategyReceived; //if (usingLidar) //{ // lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += lidarProcessor.OnRawLidarDataReceived; // //lidar_OMD60M.OnLidarDecodedFrameEvent += absolutePositionEstimator.OnRawLidarDataReceived; // lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += localWorldMapManager.OnRawLidarDataReceived; // lidarProcessor.OnLidarObjectProcessedEvent += localWorldMapManager.OnLidarObjectsReceived; //} if (usingLidar) { lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += perceptionManager.OnRawLidarDataReceived; lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += localWorldMapManager.OnRawLidarDataReceived; lidarProcessor.OnLidarProcessedEvent += localWorldMapManager.OnProcessedLidarDataReceived; } //Events de recording if (usingLogging) { //lidar_OMD60M_UDP.OnLidarDecodedFrameEvent += logRecorder.OnRawLidarDataReceived; lidar_OMD60M_TCP.OnLidarDecodedFrameEvent += logRecorder.OnRawLidarDataReceived; omniCamera.BitmapFishEyeImageEvent += logRecorder.OnBitmapImageReceived; imuProcessor.OnIMUProcessedDataGeneratedEvent += logRecorder.OnIMURawDataReceived; robotMsgProcessor.OnSpeedPolarOdometryFromRobotEvent += logRecorder.OnPolarSpeedDataReceived; //omniCamera.OpenCvMatImageEvent += logRecorder.OnOpenCVMatImageReceived; } if (usingLogReplay) { logReplay.OnLidarEvent += perceptionManager.OnRawLidarDataReceived; //logReplay.OnCameraImageEvent += imageProcessingPositionFromOmniCamera.ProcessOpenCvMatImage; //logReplay.OnCameraImageEvent += absolutePositionEstimator.AbsolutePositionEvaluation; //lidarProcessor.OnLidarObjectProcessedEvent += localWorldMapManager.OnLidarObjectsReceived; } //Timer de stratégie timerStrategie = new HighFreqTimer(0.5); timerStrategie.Tick += TimerStrategie_Tick; timerStrategie.Start(); lock (ExitLock) { // Do whatever setup code you need here // once we are done wait Monitor.Wait(ExitLock); } }
public PhysicalSimulator() { highFrequencyTimer = new HighFreqTimer(fSampling); highFrequencyTimer.Tick += HighFrequencyTimer_Tick; highFrequencyTimer.Start(); }