public void OnPhysicalPositionReceived(object sender, LocationArgs e)
{
if (robotId == e.RobotId)
{
ActualLocation = e.Location;
CalculateGhostMovement();
}
}
public void OnPhysicalPositionReceived(object sender, LocationArgs e)
{
if (robotId == e.RobotId)
{
currentLocationRefTerrain = e.Location;
CalculateGhostPosition();
PIDPosition();
}
}
public void OnPositionRobotReceived(object sender, LocationArgs location)
{
robotCurrentLocation.X = location.Location.X;
robotCurrentLocation.Y = location.Location.Y;
robotCurrentLocation.Theta = location.Location.Theta;
robotCurrentLocation.Vx = location.Location.Vx;
robotCurrentLocation.Vy = location.Location.Vy;
robotCurrentLocation.Vtheta = location.Location.Vtheta;
}
//L'arrivée d'une nouvelle position mesurée (ou simulée) déclenche le recalcul et event de perception
public void OnPhysicalRobotPositionReceived(object sender, LocationArgs e)
{
//On calcule la perception simulée de position d'après le retour du simulateur physique directement
//On réel on utilisera la triangulation lidar et la caméra
if (robotId == e.RobotId)
{
robotPerception.robotLocation = e.Location;
GeneratePerception();
}
}
//L'arrivée d'une nouvelle position mesurée (ou simulée) déclenche le recalcul et event de perception
public void OnPhysicalRobotPositionReceived(object sender, LocationArgs e)
{
if (robotId == e.RobotId)
{
robotPerception.robotKalmanLocation = e.Location;
//On transmet la location au positionnement absolu pour qu'il puisse vérifier que la nouvelle position absolue est cohérente avec le positionnement Kalman.
absolutePositionEstimator.OnPhysicalPositionReceived(sender, e);
//On génère la perception
GeneratePerception();
}
}
/*************************************** Incoming events ***********************************/
public void PositionReceived(object sender, LocationArgs e)
{
UpdateGhost();
//AsservissementPositionSurGhost(e.Location);
//oscilloX.AddPointToLine(1, e.timeStampMs / 1000.0, e.Vx);
//oscilloTheta.AddPointToLine(1, e.timeStampMs / 1000.0, e.Vtheta);
//currentTime = e.timeStampMs / 1000.0;
//asserv2WheelsSpeedDisplay.UpdatePolarOdometrySpeed(e.Vx, e.Vtheta);
}
//Input events
public void OnPhysicalRobotPositionReceived(object sender, LocationArgs e)
{
imgSubSamplingCounter++;
//On calcule la perception simulée de position d'après le retour du simulateur physique directement
//On réel on utilisera la triangulation lidar et la caméra
if (robotId == e.RobotId)
{
//On construit les datas simulées en ajoutant du bruit et des dérives
double erreurPositionCamLidarMax = 0;// 0.5; //en metre
double erreurAngleCamLidarMax = 0;// 0.2; //en radian
double erreurVitesseLineaireOdoMax = 0;// 0.1;
double erreurVitesseAngulaireOdoMax = 0;// 0.1;
double erreurVitesseAngulaireGyroMax = 0; // 0.05;
double xCamLidarSimu = e.Location.X + (rand.NextDouble() - 0.5) * 2* erreurPositionCamLidarMax;
double yCamLidarSimu = e.Location.Y + (rand.NextDouble() - 0.5) * 2* erreurPositionCamLidarMax;
double thetaCamLidarSimu = e.Location.Theta + (rand.NextDouble() - 0.5) * erreurAngleCamLidarMax;
double VxRefRobot = e.Location.Vx * Math.Cos(e.Location.Theta) - e.Location.Vy * Math.Sin(e.Location.Theta);
double VyRefRobot = e.Location.Vx * Math.Sin(e.Location.Theta) + e.Location.Vy * Math.Cos(e.Location.Theta);
double vxOdoSimu2 = VxRefRobot + (rand.NextDouble() - 0.5) * 2 * erreurVitesseLineaireOdoMax;
double vyOdoSimu2 = VyRefRobot + (rand.NextDouble() - 0.5) * 2 * erreurVitesseLineaireOdoMax;
double vxOdoSimu = e.Location.Vx + (rand.NextDouble() - 0.5) * 2 * erreurVitesseLineaireOdoMax;
double vyOdoSimu = e.Location.Vy + (rand.NextDouble() - 0.5) * 2 * erreurVitesseLineaireOdoMax;
double vthetaOdoSimu = e.Location.Vtheta + (rand.NextDouble() - 0.5) * 2 * erreurVitesseAngulaireOdoMax;
double vthetaGyroSimu = e.Location.Vtheta + (rand.NextDouble() - 0.5) * 2 * erreurVitesseAngulaireGyroMax;
if (imgSubSamplingCounter % 5 == 0)
{
OnCamLidarSimulatedRobotPosition(robotId, xCamLidarSimu, yCamLidarSimu, thetaCamLidarSimu);
}
OnOdometrySimulatedRobotSpeed(robotId, vxOdoSimu, vyOdoSimu, vthetaOdoSimu);
OnGyroSimulatedRobotSpeed(robotId, vthetaGyroSimu);
}
}
public void OnRobotLocationArgs(object sender, LocationArgs location)
{
OnUpdateRobotLocation(location.Location);
}
public void OnPhysicalBallPositionReceived(object sender, LocationArgs e)
{
//On calcule la perception simulée de position balle d'après le retour du simulateur physique directement
//On réel on utilisera la caméra
robotPerception.ballLocation = e.Location;
}
public void DestinationReceived(object sender, LocationArgs e)
{
UpdateDestination(new PointD(e.Location.X, e.Location.Y));
}
/// <summary>
/// Fetch all data from db
/// </summary>
public void Fetch()
{
using (var fr = new CustomFileReader())
{
if (!fr.Open(FilePath))
{
Console.WriteLine("Не получилось открыть запрашиваемый файл");
return;
}
var headerBuffer = new byte[60];
fr.Read(headerBuffer, 0, 60);
var recordCount = BitConverter.ToInt32(headerBuffer, 44);
ThreadPool.SetMaxThreads(Environment.ProcessorCount, Environment.ProcessorCount);
// Async fetch for ip locations
var ipLocWaiter = new ManualResetEventSlim();
var ipLocations = new IpLocation[recordCount];
var offset = 20 * recordCount;
var buffer = new byte[offset];
fr.Read(buffer, 0, offset);
ThreadPool.QueueUserWorkItem(FetchIpLocations, new ThreadParams<IpLocation>
{
Buffer = buffer,
RecordCount = recordCount,
Waiter = ipLocWaiter,
Result = ipLocations
});
// Async multithreaded fetch for ip locations
int locWaiterThreadCount = Environment.ProcessorCount;
var locWaiter = new Dictionary<int, LocationArgs>();
for (int i = 0; i < locWaiterThreadCount; i++)
{
locWaiter[i] = new LocationArgs(recordCount / locWaiterThreadCount);
}
offset = 96 * recordCount;
for (int i = 0; i < locWaiterThreadCount; i++)
{
buffer = new byte[offset / locWaiterThreadCount];
fr.Read(buffer, 0, offset / locWaiterThreadCount);
ThreadPool.QueueUserWorkItem(FetchLocations, new ThreadParams<Location>
{
Buffer = buffer,
RecordCount = recordCount / locWaiterThreadCount,
Waiter = locWaiter[i].Waiter,
Result = locWaiter[i].Data
});
}
// Async fetch for indexes
offset = 4 * recordCount;
buffer = new byte[offset];
fr.Read(buffer, 0, offset);
var indexWaiter = new ManualResetEventSlim();
var indexes = new uint[recordCount];
ThreadPool.QueueUserWorkItem(FetchIndexes, new ThreadParams<uint>
{
Buffer = buffer,
RecordCount = recordCount,
Waiter = indexWaiter,
Result = indexes
});
// Fetch for header
FetchHeader(headerBuffer, recordCount);
ipLocWaiter.Wait();
indexWaiter.Wait();
var locations = new List<Location>(recordCount);
for (int i = 0; i < locWaiterThreadCount; i++)
{
var locationArgs = locWaiter[i];
locationArgs.Waiter.Wait();
locations.InsertRange(locWaiterThreadCount * i, locationArgs.Data);
}
IpLocations = ipLocations;
Locations = locations
.OrderBy(l => l.City);
Indexes = indexes;
}
}
public void OnLocalWorldMapRobotUpdate(object sender, LocationArgs location)
{
worldMapDisplayWaypoint.UpdateRobotLocation(location.RobotId, location.Location);
worldMapDisplayStrategy.UpdateRobotLocation(location.RobotId, location.Location);
}
