示例#1
0
 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;
        }
示例#4
0
 //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();
     }
 }
示例#5
0
 //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();
     }
 }
示例#6
0
        /*************************************** 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);
        }
示例#7
0
        //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);
            }
        }
示例#8
0
 public void OnRobotLocationArgs(object sender, LocationArgs location)
 {
     OnUpdateRobotLocation(location.Location);
 }
示例#9
0
 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;
 }
示例#10
0
 public void DestinationReceived(object sender, LocationArgs e)
 {
     UpdateDestination(new PointD(e.Location.X, e.Location.Y));
 }
示例#11
0
        /// <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;
            }

        }
示例#12
0
 public void OnLocalWorldMapRobotUpdate(object sender, LocationArgs location)
 {
     worldMapDisplayWaypoint.UpdateRobotLocation(location.RobotId, location.Location);
     worldMapDisplayStrategy.UpdateRobotLocation(location.RobotId, location.Location);
 }