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;
}