public virtual void OnLocalWorldMapBypass(LocalWorldMap map) { var handler = OnLocalWorldMapBypassEvent; if (handler != null) { handler(this, new LocalWorldMapArgs { LocalWorldMap = map }); } }
public virtual void OnLocalWorldMapToGlobalWorldMapGenerator(LocalWorldMap data) { var handler = OnLocalWorldMapToGlobalWorldMapGeneratorEvent; if (handler != null) { handler(this, new LocalWorldMapArgs { LocalWorldMap = data }); } }
public virtual void OnLocalWorldMapForDisplayOnly(LocalWorldMap map) { var handler = OnLocalWorldMapForDisplayOnlyEvent; if (handler != null) { handler(this, new LocalWorldMapArgs { LocalWorldMap = map }); } }
public virtual void OnLocalWorldMap(LocalWorldMap localWorldMap) { var handler = OnLocalWorldMapEvent; if (handler != null) { handler(this, new LocalWorldMapArgs { RobotId = this.RobotId, TeamId = this.TeamId, LocalWorldMap = this.localWorldMap }); } }
public void UpdateLocalWorldMap(int robotId, LocalWorldMap localWorldMap) { UpdateRobotLocation(robotId, localWorldMap.robotLocation); UpdateRobotDestination(robotId, localWorldMap.destinationLocation); UpdateRobotWaypoint(robotId, localWorldMap.waypointLocation); if (localWorldMap.heatMap != null) { UpdateHeatMap(robotId, localWorldMap.heatMap.BaseHeatMapData); } UpdateLidarMap(robotId, localWorldMap.lidarMap); UpdateBallLocation(localWorldMap.ballLocation); }
public StrategyEurobot(int robotId, int teamId, string multicastIpAddress) : base(robotId, teamId, multicastIpAddress) { taskDemoMove = new TaskDemoMove(this); taskDemoMessage = new TaskDemoMessage(this); taskDestination = new TaskDestination(this); this.robotId = robotId; this.teamId = teamId; localWorldMap = new LocalWorldMap(robotId, teamId); GhostTimer = new Timer(20); GhostTimer.Elapsed += OnGhostTimerCalculationOrder; }
private void AddOrUpdateLocalWorldMap(int robotId, int teamId, LocalWorldMap localWorldMap) { lock (localWorldMapDictionary) { if (localWorldMapDictionary.ContainsKey(robotId)) { localWorldMapDictionary[robotId] = localWorldMap; } else { localWorldMapDictionary.Add(robotId, localWorldMap); } } }
public void OnPerceptionReceived(object sender, EventArgsLibrary.PerceptionArgs e) { //PerceptionMonitor.PerceptionReceived(); if (localWorldMap == null) { return; } if (localWorldMap.RobotId == e.RobotId) { //On ajoute les infos à la Local World Map localWorldMap.robotLocation = e.Perception.robotKalmanLocation; localWorldMap.obstaclesLocationList = e.Perception.obstaclesLocationList; localWorldMap.ballLocationList = e.Perception.ballLocationList; //On recopie les infos de la local World Map dans la structure de transfert (sans ce qui coute cher : heatmaps, lidarpoints...) LocalWorldMap transferLocalWorldMap = new LocalWorldMap(); transferLocalWorldMap.RobotId = localWorldMap.RobotId; transferLocalWorldMap.TeamId = localWorldMap.TeamId; transferLocalWorldMap.robotLocation = localWorldMap.robotLocation; transferLocalWorldMap.destinationLocation = localWorldMap.destinationLocation; transferLocalWorldMap.waypointLocation = localWorldMap.waypointLocation; transferLocalWorldMap.robotGhostLocation = localWorldMap.robotGhostLocation; transferLocalWorldMap.robotRole = localWorldMap.robotRole; transferLocalWorldMap.ballHandlingState = localWorldMap.ballHandlingState; transferLocalWorldMap.messageDisplay = localWorldMap.messageDisplay; transferLocalWorldMap.playingSide = localWorldMap.playingSide; transferLocalWorldMap.obstaclesLocationList = localWorldMap.obstaclesLocationList; transferLocalWorldMap.ballLocationList = localWorldMap.ballLocationList; if (transferLocalWorldMap.robotLocation != null) { //if (bypassMulticastUdp) //{ // OnLocalWorldMapForDisplayOnly(localWorldMap); //Pour affichage uniquement, sinon transmission radio en, multicast //} //else { var s = ZeroFormatterSerializer.Serialize <WorldMap.ZeroFormatterMsg>(transferLocalWorldMap); OnMulticastSendLocalWorldMapCommand(s); //Retiré pour test de robustesse, mais nécessaire à la RoboCup //ATTENTION : appel douteux... OnLocalWorldMapForDisplayOnly(localWorldMap); //Pour affichage uniquement, sinon transmission radio en, multicast //LWMEmiseMonitoring.LWMEmiseMonitor(s.Length); } } } }
public void OnPerceptionReceived(object sender, EventArgsLibrary.PerceptionArgs e) { //PerceptionMonitor.PerceptionReceived(); if (localWorldMap == null) { return; } if (localWorldMap.RobotId == e.RobotId) { //On ajoute les infos à la Local World Map localWorldMap.robotLocation = e.Perception.robotKalmanLocation; localWorldMap.obstaclesLocationList = e.Perception.obstaclesLocationList; localWorldMap.ballLocationList = e.Perception.ballLocationList; //On recopie les infos de la local World Map dans la structure de transfert (sans ce qui coute cher : heatmaps, lidarpoints...) LocalWorldMap transferLocalWorldMap = new LocalWorldMap(); transferLocalWorldMap.RobotId = localWorldMap.RobotId; transferLocalWorldMap.TeamId = localWorldMap.TeamId; transferLocalWorldMap.robotLocation = localWorldMap.robotLocation; transferLocalWorldMap.destinationLocation = localWorldMap.destinationLocation; transferLocalWorldMap.waypointLocation = localWorldMap.waypointLocation; transferLocalWorldMap.robotGhostLocation = localWorldMap.robotGhostLocation; transferLocalWorldMap.robotRole = localWorldMap.robotRole; transferLocalWorldMap.ballHandlingState = localWorldMap.ballHandlingState; transferLocalWorldMap.messageDisplay = localWorldMap.messageDisplay; transferLocalWorldMap.playingSide = localWorldMap.playingSide; transferLocalWorldMap.obstaclesLocationList = localWorldMap.obstaclesLocationList; transferLocalWorldMap.ballLocationList = localWorldMap.ballLocationList; if (transferLocalWorldMap.robotLocation != null) { var s = ZeroFormatterSerializer.Serialize <WorldMap.ZeroFormatterMsg>(transferLocalWorldMap); OnMulticastSendLocalWorldMapCommand(s); //Envoi à destination des autres robots en multicast OnLocalWorldMapToGlobalWorldMapGenerator(transferLocalWorldMap); //Envoi à destination du robot lui même en direct OnLocalWorldMapForDisplayOnly(localWorldMap); //Pour affichage uniquement, sinon transmission radio en, multicast //LWMEmiseMonitoring.LWMEmiseMonitor(s.Length); } } }
public void OnMulticastDataReceived(object sender, EventArgsLibrary.DataReceivedArgs e) { lock (e.Data) { var deserialization = ZeroFormatterSerializer.Deserialize <ZeroFormatterMsg>(e.Data); switch (deserialization.Type) { case ZeroFormatterMsgType.LocalWM: LocalWorldMap lwm = (LocalWorldMap)deserialization; //if (Id == lwm.RobotId) { OnLocalWorldMap(lwm); //UdpMonitor.LWMReceived(e.Data.Length); } break; case ZeroFormatterMsgType.GlobalWM: GlobalWorldMap gwm = (GlobalWorldMap)deserialization; //UdpMonitor.GWMReceived(e.Data.Length); OnGlobalWorldMap(gwm); break; case ZeroFormatterMsgType.RefBoxMsg: RefBoxMessage refBoxMsg = (RefBoxMessage)deserialization; OnRefBoxMessage(refBoxMsg); break; default: break; } //var compressedData = e.Data; //Console.WriteLine(string.Format("Received bytes: {0}", compressedData.Length)); //string uncompressedTextData = "";// Zip.UnzipText(compressedData); // //Console.WriteLine(string.Format("Decompressed bytes: {0}", uncompressedTextData.Length)); //JObject obj = JObject.Parse(Encoding.Default.GetString(e.Data)); //try //{ // switch ((int)obj["Type"]) // { // case (int)WorldMapType.LocalWM: // LocalWorldMap lwm = obj.ToObject<LocalWorldMap>(); // OnLocalWorldMap(lwm); // break; // case (int)WorldMapType.GlobalWM: // GlobalWorldMap gwm = obj.ToObject<GlobalWorldMap>(); // OnGlobalWorldMap(gwm); // break; // default: // break; // } //} //catch //{ // Console.WriteLine("Exception Message non décodable en json : UdpMulticastInterpreter"); //} } }
static void Main(string[] args) { ConsoleFormat.InitMainConsole(); /// Enregistrement de la license SciChart en début de code SciChartSurface.SetRuntimeLicenseKey(ConstVar.SCICHART_RUNTIME_KEY); ConsoleFormat.SetupScichartLicenceKey(); /// Initialisation des modules utilisés dans le robot int robotId = (int)RobotId.Robot1; int teamId = (int)TeamId.Team1; usbDriver = new USBVendor(); msgDecoder = new MsgDecoder(); msgEncoder = new MsgEncoder(); msgGenerator = new MsgGenerator(); msgProcessor = new MsgProcessor(robotId, competition); ConsoleFormat.SetupAllCommunication(); xBoxManette = new XBoxController(robotId); ConsoleFormat.SetupXboxController(); logRecorder = new LogRecorder(); logReplay = new LogReplay(); strategyManager = new StrategyEurobot(robotId, teamId, "224.16.32.79"); #region Communication to Low Lvl /// Création des liens entre module, sauf depuis et vers l'interface graphique usbDriver.OnUSBuffReceivedEvent += msgDecoder.BuffReceived; // Transmission des messages reçus par l'USB au Message Decoder msgDecoder.OnCorrectMessageReceivedEvent += msgProcessor.ProcessRobotDecodedMessage; // Transmission les messages décodés par le Message Decoder au Message Processor msgGenerator.OnMessageToRobotGeneratedEvent += msgEncoder.EncodeAndSendMessage; // Envoi des messages du générateur de message à l'encoder msgEncoder.OnSendMessageEvent += usbDriver.SendUSBMessage; // Envoi des messages en USB depuis le message encoder #endregion #region Console // Control: bool hex_viewer = false; bool hex_sender = false; bool hex_viewer_error = true; bool hex_sender_error = true; bool hex_processor = false; bool hex_generator = false; #region USB Vendor usbDriver.OnDeviceAddedEvent += ConsoleFormat.PrintNewDeviceAdded; usbDriver.OnDeviceRemovedEvent += ConsoleFormat.PrintDeviceRemoved; usbDriver.OnUsbVendorExeptionEvent += ConsoleFormat.PrintUsbErrorExeption; #endregion #region Hex Viewer if (hex_viewer) { msgDecoder.OnUnknowByteEvent += ConsoleFormat.PrintUnknowByte; msgDecoder.OnSOFByteReceivedEvent += ConsoleFormat.PrintSOF; msgDecoder.OnFunctionMSBByteReceivedEvent += ConsoleFormat.PrintFunctionMSB; msgDecoder.OnFunctionLSBByteReceivedEvent += ConsoleFormat.PrintFunctionLSB; msgDecoder.OnPayloadLenghtMSBByteReceivedEvent += ConsoleFormat.PrintLenghtMSB; msgDecoder.OnPayloadLenghtLSBByteReceivedEvent += ConsoleFormat.PrintLenghtLSB; msgDecoder.OnPayloadByteReceivedEvent += ConsoleFormat.PrintPayloadByte; msgDecoder.OnCorrectMessageReceivedEvent += ConsoleFormat.PrintCorrectChecksum; msgDecoder.OnErrorMessageReceivedEvent += ConsoleFormat.PrintWrongChecksum; } #endregion #region Hex Viewer Error if (hex_viewer_error) { msgDecoder.OnOverLenghtMessageEvent += ConsoleFormat.PrintOverLenghtWarning; msgDecoder.OnUnknowFunctionEvent += ConsoleFormat.PrintUnknowFunctionReceived; msgDecoder.OnWrongLenghtFunctionEvent += ConsoleFormat.PrintWrongFonctionLenghtReceived; } #endregion #region Hex Sender if (hex_sender) { msgEncoder.OnSendMessageByteEvent += ConsoleFormat.PrintSendMsg; } #endregion #region Hex Sender Error if (hex_sender_error) { msgEncoder.OnSerialDisconnectedEvent += ConsoleFormat.PrintOnSerialDisconnectedError; msgEncoder.OnUnknownFunctionSentEvent += ConsoleFormat.PrintUnknowFunctionSent; msgEncoder.OnWrongPayloadSentEvent += ConsoleFormat.PrintWrongFunctionLenghtSent; } #endregion #endregion #region Lidar lidar = new SickLidar(17422959); // 18110177 lidarProcess = new LidarProcess(robotId, teamId); lidar.OnLidarDeviceConnectedEvent += lidarProcess.OnNewLidarConnected; lidar.OnLidarDeviceConnectedEvent += ConsoleFormat.NewLidarDeviceConnected; lidar.PointsAvailable += lidarProcess.OnRawPointAvailable; lidar.Start(); #endregion lidarProcess.OnRawLidarDataEvent += logRecorder.OnRawLidarDataReceived; msgProcessor.OnSpeedPolarOdometryFromRobotEvent += logRecorder.OnPolarSpeedDataReceived; #region Local World Map localWorldMap = new LocalWorldMap(robotId, teamId); localWorldMap.OnUpdateRobotLocationEvent += lidarProcess.OnRobotLocation; lidarProcess.OnRawLidarPointPolarEvent += localWorldMap.OnLidarRawPointReceived; lidarProcess.OnProcessLidarPolarDataEvent += localWorldMap.OnLidarProcessedPointReceived; lidarProcess.OnProcessLidarLineDataEvent += localWorldMap.OnLidarProcessedLineReceived; lidarProcess.OnProcessLidarCupDataEvent += localWorldMap.OnLidarProcessedCupReceived; lidarProcess.OnLidarSetupRobotLocationEvent += localWorldMap.OnRobotLocation; //lidarProcess.OnProcessLidarObjectsDataEvent += localWorldMap.OnLidarProcesObjectsReceived; localWorldMap.OnLocalWorldMapEvent += strategyManager.OnLocalWorldMapReceived; localWorldMap.Init(); #endregion #region Position2Wheels positioning2Wheels = new Positioning2Wheels(robotId); msgProcessor.OnSpeedPolarOdometryFromRobotEvent += positioning2Wheels.OnOdometryRobotSpeedReceived; positioning2Wheels.OnCalculatedLocationEvent += localWorldMap.OnRobotLocationArgs; #endregion #region TrajectoryPlanner trajectoryPlanner = new TrajectoryPlanner(robotId); trajectoryPlanner.OnNewGhostLocationEvent += localWorldMap.OnGhostLocation; trajectoryPlanner.OnNewRobotLocationEvent += localWorldMap.OnRobotLocation; trajectoryPlanner.OnDestinationReachedEvent += strategyManager.OnGhostLocationReached; #endregion #region Strategy /!\Need to be Last /! \ strategyManager.On2WheelsToPolarMatrixSetupEvent += msgGenerator.GenerateMessage2WheelsToPolarMatrixSet; //Transmission des messages de set-up de la matrice de transformation moteurindepeandt -> polaire en embarqué strategyManager.On2WheelsAngleSetupEvent += msgGenerator.GenerateMessage2WheelsAngleSet; //Transmission des messages de set-up de la config angulaire des roues en embarqué strategyManager.OnOdometryPointToMeterSetupEvent += msgGenerator.GenerateMessageOdometryPointToMeter; //Transmission des messages de set-up du coeff pointToMeter en embarqué strategyManager.OnSetAsservissementModeEvent += msgGenerator.GenerateMessageSetAsservissementMode; strategyManager.OnDestinationReachedEvent += localWorldMap.OnDestinationReached; strategyManager.OnWaypointsReachedEvent += localWorldMap.OnWaypointReached; strategyManager.OnSetActualLocationEvent += trajectoryPlanner.OnUpdateActualLocation; strategyManager.OnSetWantedLocationEvent += trajectoryPlanner.OnUpdateWantedDestination; strategyManager.OnGhostCalculationBeginEvent += trajectoryPlanner.OnLaunchCalculation; strategyManager.OnSetNewWaypointEvent += localWorldMap.AddNewWaypointsEvent; strategyManager.OnSetNewDestinationEvent += localWorldMap.SetDestinationLocationEvent; strategyManager.OnUpdateGhostCalculationOrderEvent += trajectoryPlanner.OnCalculateGhostMovement; ConsoleFormat.PrintStrategyBoot(); strategyManager.InitStrategy(); //à faire après avoir abonné les events ! #endregion if (usingMatchDisplay) { StartMatchInterface(); } else { StartRobotInterface(); } ConsoleFormat.EndMainBootSequence(); while (!exitSystem) { Thread.Sleep(500); } }
public LocalWorldMapManager(int robotId, int teamId) { RobotId = robotId; TeamId = teamId; localWorldMap = new LocalWorldMap(); }
public void OnLocalWorldMapWayPointEvent(object sender, LocalWorldMap e) { worldMapDisplayWaypoint.UpdateLocalWorldMap(e); }
public void OnLocalWorldMapStrategyEvent(object sender, LocalWorldMap e) { worldMapDisplayStrategy.UpdateLocalWorldMap(e); }