Esempio n. 1
0
        public void UpdateCarPosition(Map baseMap, List<ExtractedGlyphData> glyphs)
        {
            IDictionary<string, PositionAndOrientation> markersPositions = markerFinder_.FindMarkers(glyphs);

            if (markersPositions.ContainsKey("car") && markersPositions.Count > 1 &&
                markersPositions.All(x => x.Key == "car" || baseMap.markers.ContainsKey(x.Key)) &&
                baseMap.markers.All(x => markersPositions.ContainsKey(x.Key)))
            {
                PositionAndOrientation carPosition = markersPositions["car"];
                IDictionary<string, PositionAndOrientation> stableMarkersPosition =
                    markersPositions.Where(x => x.Key.StartsWith("static")).ToDictionary(x => x.Key, x => x.Value);

                double averageAngleChange =
                    stableMarkersPosition.Average(x => x.Value.angle - baseMap.markers[x.Key].angle);
                IDictionary<string, PositionAndOrientation> stableMarkersPositionWithAngleCorrection =
                    stableMarkersPosition.ToDictionary(x => x.Key,
                        x => TransformPositionOnAngleChange(x.Value, -averageAngleChange, baseMap.mapWidth, baseMap.mapHeight));
                double averageXChange =
                    stableMarkersPositionWithAngleCorrection.Average(x => x.Value.x - baseMap.markers[x.Key].x);
                double averageYChange =
                    stableMarkersPositionWithAngleCorrection.Average(x => x.Value.y - baseMap.markers[x.Key].y);

                PositionAndOrientation correctedCarPosition =
                    TransformPositionOnAngleChange(carPosition, -averageAngleChange, baseMap.mapWidth, baseMap.mapHeight);
                correctedCarPosition.x -= averageXChange;
                correctedCarPosition.y -= averageYChange;

                baseMap.car = correctedCarPosition;
                Console.WriteLine("car position has been updated");
            }
            else
            {
                Console.WriteLine("car position cannot be updated basing on current frame");
            }
        }
Esempio n. 2
0
        public void TrackPlannerFind360TurnTrackTest()
        {
            TrackPlanner planner = new TrackPlanner(
                locationTolerance: 9, angleTolerance: 19.0d,
                positionStep: 10.0d, angleStep: 20.0d,
                mapSizeX: 1000.0d, mapSizeY: 1000.0d);
            Map map = new Map(1000, 1000);

            map.car = new PositionAndOrientation(_x: 500.0d, _y: 500.0d, _angle: 90.0d);
            map.parking = new PositionAndOrientation(_x: 500.0d, _y: 500.0d, _angle: 90.0d);

            List<PositionAndOrientation> track = planner.PlanTrack(map);
            Assert.IsTrue(track.Count > 10); // its really bad assert, but still better than nothing
        }
Esempio n. 3
0
        public void TrackPlannerFindSmallForwardTrackTest()
        {
            TrackPlanner planner = new TrackPlanner(
                locationTolerance: 9, angleTolerance: 19.0d,
                positionStep: 10.0d, angleStep: 20.0d,
                mapSizeX: 1000.0d, mapSizeY: 1000.0d);

            Map map = new Map(1000, 1000);

            map.car = new PositionAndOrientation(_x: 500.0d, _y: 500.0d, _angle: 90.0d);
            map.parking = new PositionAndOrientation(_x: 500.0d, _y: 510.0d, _angle: 90.0d);

            List<PositionAndOrientation> track = planner.PlanTrack(map);
            Assert.AreEqual(2, track.Count);
        }
Esempio n. 4
0
        public void TrackPlannerFind90TurnTrackTest()
        {
            TrackPlanner planner = new TrackPlanner(
                locationTolerance: 9, angleTolerance: 19.0d,
                positionStep: 10.0d, angleStep: 20.0d,
                mapSizeX: 1000.0d, mapSizeY: 1000.0d);

            Map map = new Map(1000, 1000);

            map.car = new PositionAndOrientation(_x: 500.0d, _y: 500.0d, _angle: 90.0d);
            map.parking = new PositionAndOrientation(_x: 600.0d, _y: 600.0d, _angle: 0.0d);

            List<PositionAndOrientation> track = planner.PlanTrack(map);
            Assert.IsTrue(track.Count > 10);  // coz it not a bad expectation ;)
        }
Esempio n. 5
0
        public CarDriver(Map _baseMap, List<PositionAndOrientation> _track)
        {
            if (_track.Count < 2)
                throw new ArgumentException();
            if (_baseMap.car == null)
                throw new ArgumentException();

            baseMap = _baseMap;
            track = new LinkedList<PositionAndOrientation>(_track);
            distanceRegulator = new PIDRegulator(distanceRegulatorSettings, "Car Driver distance regulator");
            distanceRegulator.targetValue = 0.0d;
            angleRegulator = new PIDRegulator(angleRegulatorSettings, "Car Driver ANGLE regulator");
            angleRegulator.targetValue = 0.0d;

            currentPosition = track.First;
        }
Esempio n. 6
0
        public Form1()
        {
            TrackPlanner planner = new TrackPlanner(
            locationTolerance: 9, angleTolerance: 19.0d,
            positionStep: 10.0d, angleStep: 20.0d,
            mapSizeX: 1000.0d, mapSizeY: 1000.0d);
            Map map = new Map(1000, 1000);

            map.car = new PositionAndOrientation(_x: 100.0d, _y: 100.0d, _angle: 90.0d);
            map.parking = new PositionAndOrientation(_x: 900.0d, _y: 900.0d, _angle: 90.0d);

            List<PositionAndOrientation> track = planner.PlanTrack(map);

            Application.Exit();
            InitializeComponent();
        }
Esempio n. 7
0
        public Map BuildMap(System.Drawing.Bitmap bitmap, List<ExtractedGlyphData> glyphs)
        {
            Map map = new Map(bitmap.Size.Width, bitmap.Size.Height);

            IDictionary<string, PositionAndOrientation> markersPositions = markerFinder_.FindMarkers(glyphs);

            if (markersPositions.ContainsKey("car") && markersPositions.Count > 1)
            {
                map.car = markersPositions["car"];
                map.markers = markersPositions.Where(x => x.Key.StartsWith("static")).ToDictionary(x => x.Key, x => x.Value);
                map.obstacles = obstaclesFinder_.FindObstacles(bitmap);
            }
            else
            {
                //System.Windows.Forms.MessageBox.Show("not sufficient informations to build a map");

                return null;
            }

            return map;
        }
Esempio n. 8
0
        public CarSteering CalculateCarSteering(Map currentMap)
        {
            CarSteering steering = new CarSteering();

            currentPosition = GetNewCurrentPosition(currentMap, currentPosition);
            currentMode = GetNewMode(currentPosition);

            if (currentMode == Mode.ON_TRACK)
            {
                steering.speed = DEFAULT_SPEED_IN_MS;
                steering.brake = 0.0d;
            }
            else if (currentMode == Mode.REACHED_DESTINATION)
            {
                steering.speed = 0.0d;
                steering.brake = BREAKING_POWER_ON_DESTINATION;
            }
            else
            {
                throw new ApplicationException("unknown mode!");
            }

            double distanceAndSide = CalculateDistanceAndSideBetweenLineAndPoint(currentPosition.Value, currentMap.car);
            double distanceRegulatorValue = distanceRegulator.ProvideObjectCurrentValueToRegulator(distanceAndSide);
            double angleRegulatorValue = angleRegulator.ProvideObjectCurrentValueToRegulator(currentPosition.Value.angle);
            steering.angle = distanceRegulatorValue + angleRegulatorValue;

            return steering;
        }
Esempio n. 9
0
        private LinkedListNode<PositionAndOrientation> GetNewCurrentPosition(Map map,
            LinkedListNode<PositionAndOrientation> currentPosition)
        {
            if (currentPosition.Next == null)
                return currentPosition;

            if (CalculateDistance(map.car, currentPosition.Value) <
                CalculateDistance(map.car, currentPosition.Next.Value))
            {
                return currentPosition;
            }
            else
            {
                Console.WriteLine("Reached checkpoint: {0}!", checkPointCounter++);
                return GetNewCurrentPosition(map, currentPosition.Next);
            }
        }
Esempio n. 10
0
        void glyphRecogniser_frameProcessed(object sender, GlyphRecognitionStudio.MainForm.FrameData frameData)
        {
            baseImage = frameData.getImage();
            this.Dispatcher.Invoke(new Action(() => image_baseImagePicker.Source = loadBitmap(baseImage)));
            if (waitingForNextBaseImage)
            {
                map = mapBuilder.BuildMap(baseImage, frameData.getGlyphs());

                if (map != null)
                {
                    this.Dispatcher.Invoke(new Action(() => TextBlock_marksInfo.Text = map.ToString()));
                    this.Dispatcher.Invoke(new Action(() => DrawMap(map)));
                    this.Dispatcher.Invoke(new Action(() => plannerBackGround.Source = loadBitmap(baseImage)));
                    this.Dispatcher.Invoke(new Action(() => Canvas_trackPlanner.UpdateLayout()));
                }
                else
                {
                    this.Dispatcher.Invoke(new Action(() => TextBlock_marksInfo.Text = "Map couldn't be build basing on current image!"));
                }

                //waitingForNextBaseImage = false;
                Console.WriteLine("New base frame acquired!");
            }
            else
            {
                if (map != null)
                {
                    mapBuilder.UpdateCarPosition(map, frameData.getGlyphs());
                    this.Dispatcher.Invoke(new Action(() => DrawMap(map)));
                    Console.WriteLine("Car position updated!");

                    if (carUnderDriving)
                    {
                        CarSteering carSteering = carDriver.CalculateCarSteering(map);
                        this.Dispatcher.Invoke(new Action(() => TextBlock_CarSteeringInformations.Text = carSteering.ToString()));

                        carController.SetTargetSpeed(carSteering.speed);
                        carController.SetTargetWheelAngle(carSteering.angle);
                        carController.OverrideTargetBrakeSetting(carSteering.brake);
                    }
                }
            }
        }
Esempio n. 11
0
        public void TrackPlannerObstacleTest()
        {
            TrackPlanner planner = new TrackPlanner(
                locationTolerance: 39, angleTolerance: 19.0d,
                positionStep: 40.0, angleStep: 20.0d,
                mapSizeX: 1000.0d, mapSizeY: 1000.0d);
            Map map = new Map(1000, 1000);

            map.car = new PositionAndOrientation(_x: 500.0, _y: 100.0d, _angle: 90.0d);
            map.parking = new PositionAndOrientation(_x: 500.0, _y: 900.0, _angle: 90.0d);
            map.obstacles.Add(new Rectangle(350, 500, 300, 50));

            List<PositionAndOrientation> track = planner.PlanTrack(map);
            Assert.IsTrue(track.Count > 20);
        }
Esempio n. 12
0
        private void DrawObstacles(Map map)
        {
            foreach (var obstacle in lastObstacles)
            {
                Canvas_trackPlanner.Children.Remove(obstacle);
            }
            lastObstacles.Clear();

            foreach (var obstacle in map.obstacles)
            {
                System.Windows.Shapes.Rectangle rect = new System.Windows.Shapes.Rectangle();
                rect.Stroke = new SolidColorBrush(Colors.Red);
                rect.Width = obstacle.Width;
                rect.Height = obstacle.Height;
                Canvas.SetLeft(rect, obstacle.X);
                Canvas.SetTop(rect, obstacle.Y);
                rect.StrokeThickness = 3;

                Canvas_trackPlanner.Children.Add(rect);
                lastObstacles.Add(rect);
            }
        }
Esempio n. 13
0
 private void DrawMap(Map map)
 {
     DrawBorder(map);
     DrawCar(map);
     DrawParking(map);
     DrawObstacles(map);
     Canvas_trackPlanner.UpdateLayout();
 }
Esempio n. 14
0
        private void DrawCar(Map map)
        {
            if (lastCar != null)
                Canvas_trackPlanner.Children.Remove(lastCar);

            const double CAR_WIDTH = 55;
            const double CAR_HEIGHT = 25;
            const double FRONT_SHOWER_LENTH = 10;

            System.Windows.Shapes.Polyline car = new Polyline();
            car.StrokeThickness = 7;
            car.Stroke = new SolidColorBrush(Colors.Red);
            car.Points = new PointCollection() {
                new Point(map.car.x + CAR_WIDTH / 2, map.car.y + CAR_HEIGHT / 2),
                new Point(map.car.x - CAR_WIDTH / 2, map.car.y + CAR_HEIGHT / 2),
                new Point(map.car.x - CAR_WIDTH / 2, map.car.y - CAR_HEIGHT / 2),
                new Point(map.car.x + CAR_WIDTH / 2, map.car.y - CAR_HEIGHT / 2),
                new Point(map.car.x + CAR_WIDTH / 2, map.car.y),
                new Point(map.car.x + CAR_WIDTH + FRONT_SHOWER_LENTH / 2, map.car.y),
                new Point(map.car.x + CAR_WIDTH / 2, map.car.y),
                new Point(map.car.x + CAR_WIDTH / 2, map.car.y + CAR_HEIGHT / 2)};

            RotateTransform rt = new RotateTransform(map.car.angle, map.car.x, map.car.y);

            car.Points = new PointCollection(car.Points.Select(x => rt.Transform(x)));
            Canvas_trackPlanner.Children.Add(car);

            lastCar = car;
        }
Esempio n. 15
0
        private void DrawBorder(Map map)
        {
            if (lastBorder != null)
                Canvas_trackPlanner.Children.Remove(lastBorder);

            System.Windows.Shapes.Rectangle border = new System.Windows.Shapes.Rectangle();
            border.Stroke = new SolidColorBrush(Colors.Black);
            border.Width = map.mapWidth;
            border.Height = map.mapHeight;
            Canvas.SetLeft(border, 0);
            Canvas.SetTop(border, 0);
            border.StrokeThickness = 5;

            Canvas_trackPlanner.Children.Add(border);
            lastBorder = border;
        }
Esempio n. 16
0
        public void PrepareTracks(Map map)
        {
            seen_.Clear();

            preparedMap_ = map; // hack a bit - we r holding state here

            obstacles_ = map.obstacles;

            Queue<BFSNode> frontier = new Queue<BFSNode>();
            startPoint_ = new BFSNode(map.car, null);
            frontier.Enqueue(startPoint_);

            PositionAndOrientation target = map.parking;

            while (frontier.Count != 0)
            {
                BFSNode curr = frontier.Dequeue();

                List<BFSNode> succesors = generateSuccessors(curr);

                succesors.ForEach(x => frontier.Enqueue(x));
                succesors.ForEach(x => seen_.Add(x));
            }
        }
Esempio n. 17
0
        //TODO: add version with recalc from 80% with better accuaracy
        /// <summary>
        /// it finishes when gets to target
        /// </summary>
        /// <param name="map"></param>
        /// <returns></returns>
        public List<PositionAndOrientation> PlanTrack(Map map)
        {
            if (seen_.GetAllObjects().Count != 0)
                throw new ApplicationException();

            obstacles_ = map.obstacles;

            Queue<BFSNode> frontier = new Queue<BFSNode>();
            BFSNode startPoint = new BFSNode(map.car, null);
            frontier.Enqueue(startPoint);

            PositionAndOrientation target = map.parking;

            while (frontier.Count != 0)
            {
                BFSNode curr = frontier.Dequeue();

                List<BFSNode> succesors = generateSuccessors(curr);

                foreach (BFSNode s in succesors)
                {
                    if (ArePointsSame(s.position, target))
                        return GetPathBetweenPoints(startPoint, s);
                }

                succesors.ForEach(x => frontier.Enqueue(x));
                succesors.ForEach(x => seen_.Add(x));
            }

            return new List<PositionAndOrientation>();  // couldn't find result - returning empty list
        }
Esempio n. 18
0
        private void DrawParking(Map map)
        {
            if (map.parking != null)
            {
                const double parkingSizeX = 38;
                const double parkingSizeY = 70;

                System.Windows.Shapes.Rectangle parking = new System.Windows.Shapes.Rectangle();
                parking.Stroke = new SolidColorBrush(Colors.Red);
                parking.Width = parkingSizeX;
                parking.Height = parkingSizeY;
                Canvas.SetLeft(parking, map.parking.x - parkingSizeX / 2);
                Canvas.SetTop(parking, map.parking.y - parkingSizeY / 2);
                parking.StrokeThickness = 7;

                Canvas_trackPlanner.Children.Add(parking);
            }
        }