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