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 void DrawObstacle(IGraphics g, WorldTransform wt, OperationalObstacle obs)
{
IPen pen = g.CreatePen();
pen.Width = nom_pixel_width / wt.Scale;
pen.Color = classColors[(int)obs.obstacleClass];
g.DrawPolygon(pen, Utility.ToPointF(obs.poly));
if (obs.headingValid)
{
DrawingUtility.DrawArrow(g, obs.poly.GetCentroid(), Coordinates.FromAngle(obs.heading), 4, 0.75, Color.Black, wt);
}
if (ageRenderFlag || obs.ignored)
{
// draw the model confidence
string labelString = "";
if (ageRenderFlag)
{
labelString += obs.age.ToString();
}
if (obs.ignored)
{
if (labelString.Length != 0)
{
labelString += ", ";
}
labelString += "IGN";
}
SizeF stringSize = g.MeasureString(labelString, labelFont);
stringSize.Width /= wt.Scale;
stringSize.Height /= wt.Scale;
Coordinates labelPt = obs.poly.GetCentroid();
RectangleF rect = new RectangleF(Utility.ToPointF(labelPt), stringSize);
float inflateValue = 4 / wt.Scale;
rect.X -= inflateValue;
rect.Y -= inflateValue;
rect.Width += 2 / wt.Scale;
g.FillRectangle(Color.FromArgb(127, Color.White), rect);
g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(labelPt));
}
}
private void HandleDataValueMessage(Stream s)
{
// construct a binary reader
BinaryReader reader = new BinaryReader(s);
// get the sequence number
int seqNo = reader.ReadInt32();
// calculate how many packet we've missed
// we are expecting a difference of 1 in the sequence numbers
int missed = seqNo - lastSeqNo - 1;
// if the difference is less than zero, something stuff is going on
if (missed > 0 && lastSeqNo != -1)
{
missedCount += missed;
}
// set the last sequence number
lastSeqNo = seqNo;
// increment the received count
receivedCount++;
// get the data item name
string diName = reader.ReadString();
// get the time
CarTimestamp t = reader.ReadDouble();
// read the data type code
DataTypeCode dtc = (DataTypeCode)reader.ReadInt32();
// handle the type appropriately
object value = null;
switch (dtc)
{
case DataTypeCode.Double:
value = reader.ReadDouble();
break;
case DataTypeCode.Single:
value = reader.ReadSingle();
break;
case DataTypeCode.Int8:
value = reader.ReadSByte();
break;
case DataTypeCode.Int16:
value = reader.ReadInt16();
break;
case DataTypeCode.Int32:
value = reader.ReadInt32();
break;
case DataTypeCode.Int64:
value = reader.ReadInt64();
break;
case DataTypeCode.UInt8:
value = reader.ReadByte();
break;
case DataTypeCode.UInt16:
value = reader.ReadUInt16();
break;
case DataTypeCode.UInt32:
value = reader.ReadUInt32();
break;
case DataTypeCode.UInt64:
value = reader.ReadUInt64();
break;
case DataTypeCode.Boolean:
value = reader.ReadBoolean();
break;
case DataTypeCode.DateTime:
value = DateTime.FromBinary(reader.ReadInt64());
break;
case DataTypeCode.TimeSpan:
value = new TimeSpan(reader.ReadInt64());
break;
case DataTypeCode.Coordinates:
value = new Coordinates(reader.ReadDouble(), reader.ReadDouble());
break;
case DataTypeCode.Circle:
value = new Circle(reader.ReadDouble(), new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
break;
case DataTypeCode.Line:
value = new Line(new Coordinates(reader.ReadDouble(), reader.ReadDouble()), new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
break;
case DataTypeCode.LineSegment:
value = new LineSegment(new Coordinates(reader.ReadDouble(), reader.ReadDouble()), new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
break;
case DataTypeCode.Polygon: {
CoordinateMode coordMode = (CoordinateMode)reader.ReadInt32();
int count = reader.ReadInt32();
Polygon pg = new Polygon(coordMode);
for (int i = 0; i < count; i++)
{
pg.Add(new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
}
value = pg;
}
break;
case DataTypeCode.Bezier: {
value = new CubicBezier(
new Coordinates(reader.ReadDouble(), reader.ReadDouble()),
new Coordinates(reader.ReadDouble(), reader.ReadDouble()),
new Coordinates(reader.ReadDouble(), reader.ReadDouble()),
new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
}
break;
case DataTypeCode.LineList: {
LineList linelist = new LineList();
int ll_count = reader.ReadInt32();
linelist.Capacity = ll_count;
for (int i = 0; i < ll_count; i++)
{
linelist.Add(new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
}
value = linelist;
}
break;
case DataTypeCode.CoordinatesArray: {
int numPts = reader.ReadInt32();
Coordinates[] pts = new Coordinates[numPts];
for (int i = 0; i < numPts; i++)
{
pts[i] = new Coordinates(reader.ReadDouble(), reader.ReadDouble());
}
value = pts;
}
break;
case DataTypeCode.LineListArray: {
int numLineList = reader.ReadInt32();
LineList[] lineLists = new LineList[numLineList];
for (int i = 0; i < numLineList; i++)
{
int numPoints = reader.ReadInt32();
lineLists[i] = new LineList(numPoints);
for (int j = 0; j < numPoints; j++)
{
lineLists[i].Add(new Coordinates(reader.ReadDouble(), reader.ReadDouble()));
}
}
value = lineLists;
}
break;
case DataTypeCode.PolygonArray: {
int numPolys = reader.ReadInt32();
Polygon[] polys = new Polygon[numPolys];
for (int i = 0; i < numPolys; i++)
{
int numPoints = reader.ReadInt32();
polys[i] = new Polygon(numPoints);
for (int j = 0; j < numPoints; j++)
{
polys[i].Add(ReadReducedCoord(reader));
}
}
value = polys;
}
break;
case DataTypeCode.ObstacleArray: {
int numObs = reader.ReadInt32();
OperationalObstacle[] obstacles = new OperationalObstacle[numObs];
for (int i = 0; i < numObs; i++)
{
OperationalObstacle obs = new OperationalObstacle();
obs.age = reader.ReadInt32();
obs.obstacleClass = (ObstacleClass)reader.ReadInt32();
obs.ignored = reader.ReadBoolean();
obs.headingValid = reader.ReadBoolean();
obs.heading = reader.ReadDouble();
int numPoints = reader.ReadInt32();
obs.poly = new Polygon(numPoints);
for (int j = 0; j < numPoints; j++)
{
obs.poly.Add(ReadReducedCoord(reader));
}
obstacles[i] = obs;
}
value = obstacles;
}
break;
case DataTypeCode.BinarySerialized:
if (reader.ReadByte() == 0)
{
value = formatter.Deserialize(s);
}
else
{
DeflateStream ds = new DeflateStream(s, CompressionMode.Decompress, true);
value = formatter.Deserialize(ds);
}
break;
default:
// unknown type
throw new InvalidOperationException();
}
// check if there is no value
if (value == null)
{
return;
}
// check if we have the data item
IDataItemClient diObj;
if (!items.TryGetValue(diName, out diObj))
{
// build the data item
Type diGenType = typeof(DataItemClient <>);
Type diType = diGenType.MakeGenericType(value.GetType());
diObj = (IDataItemClient)Activator.CreateInstance(diType, diName);
Add(diName, diObj);
}
diObj.AddDataItem(value, t);
}
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);
}
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);
}
