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");
}
}
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
}
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);
}
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 ;)
}
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;
}
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();
}
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;
}
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;
}
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);
}
}
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);
}
}
}
}
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);
}
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);
}
}
private void DrawMap(Map map)
{
DrawBorder(map);
DrawCar(map);
DrawParking(map);
DrawObstacles(map);
Canvas_trackPlanner.UpdateLayout();
}
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;
}
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;
}
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));
}
}
//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
}
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);
}
}