private List<Obstacle> FinalizeProcessing(List<Obstacle> trackedObstacles, List<Obstacle> untrackedObstacles, CarTimestamp timestamp) { List<Obstacle> finalObstacles = new List<Obstacle>(trackedObstacles.Count+untrackedObstacles.Count); finalObstacles.AddRange(trackedObstacles); finalObstacles.AddRange(untrackedObstacles); Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles); Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles); int extraCount = 0; if (leftObstacle != null) extraCount++; if (rightObstacle != null) extraCount++; List<int> ignoredObstacles = this.lastIgnoredObstacles; OperationalObstacle[] uiObstacles = new OperationalObstacle[finalObstacles.Count + extraCount]; int i; for (i = 0; i < finalObstacles.Count; i++) { Obstacle obs = finalObstacles[i]; obs.minSpacing = min_spacing[(int)obs.obstacleClass] + ExtraSpacing; obs.desSpacing = des_spacing[(int)obs.obstacleClass] + ExtraSpacing; try { obs.cspacePolygon = Polygon.ConvexMinkowskiConvolution(conv_polygon[(int)obs.obstacleClass], obs.AvoidancePolygon); } catch (Exception) { OperationalTrace.WriteWarning("error computing minkowski convolution in finalize processing"); try { obs.cspacePolygon = obs.AvoidancePolygon.Inflate(TahoeParams.T/2.0+min_spacing[(int)obs.obstacleClass]); } catch (Exception) { obs.cspacePolygon = obs.AvoidancePolygon; } } OperationalObstacle uiObs = new OperationalObstacle(); uiObs.age = obs.age; uiObs.obstacleClass = obs.obstacleClass; uiObs.poly = obs.AvoidancePolygon; uiObs.headingValid = obs.absoluteHeadingValid; uiObs.heading = obs.absoluteHeading; if (ignoredObstacles != null) { uiObs.ignored = ignoredObstacles.Contains(obs.trackID); } uiObstacles[i] = uiObs; } if (leftObstacle != null) { OperationalObstacle uiObs = new OperationalObstacle(); uiObs.age = leftObstacle.age; uiObs.obstacleClass = leftObstacle.obstacleClass; uiObs.poly = leftObstacle.AvoidancePolygon; uiObstacles[i++] = uiObs; } if (rightObstacle != null) { OperationalObstacle uiObs = new OperationalObstacle(); uiObs.age = rightObstacle.age; uiObs.obstacleClass = rightObstacle.obstacleClass; uiObs.poly = rightObstacle.AvoidancePolygon; uiObstacles[i++] = uiObs; } Services.UIService.PushObstacles(uiObstacles, timestamp, "obstacles", true); return finalObstacles; }
public void PushObstacles(OperationalObstacle[] obstacles, CarTimestamp timestamp, string name, bool relative) { try { if (obstacles == null || obstacles.Length == 0) return; if (relative) { OperationalObstacle[] transformObstacles = new OperationalObstacle[obstacles.Length]; AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert(); timestamp = absTransform.Timestamp; for (int i = 0; i < obstacles.Length; i++) { transformObstacles[i] = obstacles[i].ShallowClone(); transformObstacles[i].poly = obstacles[i].poly.Transform(absTransform); } obstacles = transformObstacles; } Services.Dataset.ItemAs<OperationalObstacle[]>(name).Add(obstacles, timestamp); } catch (Exception) { } }
private List<Obstacle> GetPerimeterObstacles() { // transform the polygon to relative coordinates AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); Polygon relPerimeter = zonePerimeter.Transform(absTransform); LinePath relRecommendedPath = recommendedPath.Transform(absTransform); if (relPerimeter.IsCounterClockwise) { relPerimeter = relPerimeter.Reverse(); } // create a polygon for ourselves and see if we intersect any perimeters Polygon vehiclePoly = new Polygon(); vehiclePoly.Add(new Coordinates(-TahoeParams.RL, -(TahoeParams.T/2.0))); vehiclePoly.Add(new Coordinates(TahoeParams.FL, -(TahoeParams.T/2.0))); vehiclePoly.Add(new Coordinates(TahoeParams.FL, TahoeParams.T/2.0)); vehiclePoly.Add(new Coordinates(-TahoeParams.RL, TahoeParams.T/2.0)); // inflate by about 2 m vehiclePoly = vehiclePoly.Inflate(2); // test if we intersect any of the perimeter points List<Obstacle> perimeterObstacles = new List<Obstacle>(); List<OperationalObstacle> operationalObstacles = new List<OperationalObstacle>(); List<LineSegment> segments = new List<LineSegment>(); foreach (LineSegment ls1 in relPerimeter.GetSegmentEnumerator()) { segments.Clear(); if (ls1.Length > 15) { // split into multiple segment double targetLength = 10; int numSegments = (int)Math.Round(ls1.Length/targetLength); double splitLength = ls1.Length/numSegments; Coordinates pt = ls1.P0; for (int i = 0; i < numSegments; i++) { Coordinates endPoint = pt + ls1.Vector.Normalize(splitLength); LineSegment seg = new LineSegment(pt, endPoint); segments.Add(seg); pt = endPoint; } } else { segments.Add(ls1); } foreach (LineSegment ls in segments) { bool pathTooClose = false; foreach (Coordinates pt in relRecommendedPath) { Coordinates closest = ls.ClosestPoint(pt); if (closest.DistanceTo(pt) < 1) pathTooClose = true; } if (!vehiclePoly.DoesIntersect(ls) && !pathTooClose) { Obstacle obs = CreatePerimeterObstacle(ls); perimeterObstacles.Add(obs); OperationalObstacle uiobs = new OperationalObstacle(); uiobs.age = obs.age; uiobs.heading = 0; uiobs.headingValid = false; uiobs.ignored = false; uiobs.obstacleClass = obs.obstacleClass; uiobs.poly = obs.obstaclePolygon; operationalObstacles.Add(uiobs); } } } Services.UIService.PushObstacles(operationalObstacles.ToArray(), curTimestamp, "perimeter obstacles", true); return perimeterObstacles; }