/// <summary>
/// What to do when mouse moves
/// </summary>
/// <param name="e"></param>
protected override void OnMouseMove(MouseEventArgs e)
{
#region Left
if (e.Button == MouseButtons.Left)
{
// dragging vehicle
if (this.selected != null && this.selected is CarDisplayObject && isDragging && this.temporaryCoordinate.HasValue)
{
// Get the offset.
Point point = e.Location;
// new coord
Coordinates newCoord = this.transform.GetWorldPoint(new PointF(point.X, point.Y));
// calc offse
Coordinates offset = newCoord - this.temporaryCoordinate.Value;
// moving object
CarDisplayObject cdo = (CarDisplayObject)this.selected;
// check we are not tracking
if (this.tracked != null && cdo.Equals(this.tracked))
{
this.tracked = null;
}
// move
cdo.InMove(this.temporaryCoordinate.Value, offset, this.transform);
// check snap pos or heading
if (cdo.SnapHeading || cdo.SnapPosition)
{
// filter for vehicles
DisplayObjectFilter partitionDof = delegate(IDisplayObject target)
{
// check if target is network object
if (target is ArbiterLanePartition)
{
return(true);
}
else
{
return(false);
}
};
// check to see if selected a partition
HitTestResult vhcHtr = this.HitTest(transform.GetWorldPoint(new PointF(e.X, e.Y)), partitionDof);
// check hit
if (vhcHtr.Hit)
{
// get partition
ArbiterLanePartition alp = (ArbiterLanePartition)vhcHtr.DisplayObject;
// heading
Coordinates heading = alp.Vector();
// position
Coordinates closest = alp.PartitionPath.GetPoint(alp.PartitionPath.GetClosestPoint(transform.GetWorldPoint(new PointF(e.X, e.Y))));
if (cdo.SnapPosition)
{
cdo.Position = closest;
}
if (cdo.SnapHeading)
{
cdo.Heading = heading;
}
}
}
}
else if (this.selected != null && this.selected is SimObstacle && isDragging && this.temporaryCoordinate.HasValue)
{
// Get the offset.
Point point = e.Location;
// new coord
Coordinates newCoord = this.transform.GetWorldPoint(new PointF(point.X, point.Y));
// calc offse
Coordinates offset = newCoord - this.temporaryCoordinate.Value;
// moving object
SimObstacle so = (SimObstacle)this.selected;
// move
so.InMove(this.temporaryCoordinate.Value, offset, this.transform);
}
// check if user is dragging
else if (isDragging)
{
// Get the offset.
Point point = (Point)controlTag;
// Calculate change in position
double deltaX = e.X - point.X;
double deltaY = e.Y - point.Y;
// Update the world
Coordinates tempCenter = WorldTransform.CenterPoint;
tempCenter.X -= deltaX / WorldTransform.Scale;
tempCenter.Y += deltaY / WorldTransform.Scale;
WorldTransform.CenterPoint = tempCenter;
// update control
controlTag = new Point(e.X, e.Y);
}
// redraw
this.Invalidate();
}
#endregion
#region Right
else if (e.Button == MouseButtons.Right)
{
if (this.selected != null && this.selected is SimObstacle && isDragging && this.temporaryCoordinate.HasValue)
{
// Get the offset.
Point point = e.Location;
// new coord
Coordinates newCoord = this.transform.GetWorldPoint(new PointF(point.X, point.Y));
// moving object
SimObstacle so = (SimObstacle)this.selected;
// calc new rel heading
Coordinates offset = newCoord - so.Position;
// calc degree diff
//double rotDiff = offset.ToDegrees() - this.temporaryCoordinate.Value.ToDegrees();
// new head
//Coordinates newHead = so.Heading.Rotate(rotDiff);
// set
so.Heading = offset;
this.Invalidate();
}
}
#endregion
base.OnMouseMove(e);
}
private void InitializeDisplayObjects()
{
grid = new GridDisplayObject();
// add seperate from display object service
Services.DisplayObjectService.Add(grid, true);
// car object
CarDisplayObject carDisplayObject = new CarDisplayObject("tahoe", Color.SkyBlue);
carDisplayObject.AttachToVehicleState = true;
Services.DisplayObjectService.Add(carDisplayObject, true);
// tracked paths
TrackedPathDisplayObject posePath = new TrackedPathDisplayObject("paths/pose", Color.LightBlue);
disposables.Add(DataItemAttacher.Attach(posePath, OperationalInterface.Dataset.ItemAs<Coordinates>("xy")));
Services.DisplayObjectService.Add(posePath, true);
TrackedPathDisplayObject posteriorPosePath = new TrackedPathDisplayObject("paths/posterior pose", Color.DarkGreen);
disposables.Add(DataItemAttacher.Attach(posteriorPosePath, OperationalInterface.Dataset.ItemAs<Coordinates>("posterior pose")));
Services.DisplayObjectService.Add(posteriorPosePath, true);
TrackedPathDisplayObject hpPath = new TrackedPathDisplayObject("paths/hp", Color.HotPink);
disposables.Add(DataItemAttacher.Attach(hpPath, OperationalInterface.Dataset.ItemAs<Coordinates>("hp xy")));
Services.DisplayObjectService.Add(hpPath, true);
TrackedPathDisplayObject sepPath = new TrackedPathDisplayObject("paths/septentrio", Color.DarkRed);
disposables.Add(DataItemAttacher.Attach(sepPath, OperationalInterface.Dataset.ItemAs<Coordinates>("gps xy")));
Services.DisplayObjectService.Add(sepPath, true);
// path point
PointDisplayObject pathPoint = new PointDisplayObject("tracking/path point", "path point", Color.Green, ControlPointStyle.LargeX, ContentAlignment.BottomRight);
disposables.Add(DataItemAttacher.Attach(pathPoint, OperationalInterface.Dataset.ItemAs<Coordinates>("path point")));
Services.DisplayObjectService.Add(pathPoint, true);
LineListDisplayObject trackingPath = new LineListDisplayObject("tracking/tracking path", Color.DeepPink);
disposables.Add(DataItemAttacher.Attach(trackingPath, OperationalInterface.Dataset.ItemAs<LineList>("tracking path")));
Services.DisplayObjectService.Add(trackingPath, false);
// u-turn stuff
CircleDisplayObject uturnCircle = new CircleDisplayObject("u-turn/planned circle", Color.Magenta);
disposables.Add(DataItemAttacher.Attach(uturnCircle, OperationalInterface.Dataset.ItemAs<Circle>("uturn circle")));
Services.DisplayObjectService.Add(uturnCircle, true);
PointDisplayObject uturnStopPoint = new PointDisplayObject("u-turn/stop point", null, Color.Black, ControlPointStyle.SmallX, ContentAlignment.BottomRight);
disposables.Add(DataItemAttacher.Attach(uturnStopPoint, OperationalInterface.Dataset.ItemAs<Coordinates>("uturn stop point")));
Services.DisplayObjectService.Add(uturnStopPoint, true);
PolygonDisplayObject uturnPolygon = new PolygonDisplayObject("u-turn/polygon", Color.BurlyWood, false);
disposables.Add(DataItemAttacher.Attach(uturnPolygon, OperationalInterface.Dataset.ItemAs<Polygon>("uturn polygon")));
Services.DisplayObjectService.Add(uturnPolygon, true);
// original path objects (paths frank sends)
LineListDisplayObject originalPath1 = new LineListDisplayObject("planning/original path 1", Color.DarkOrchid);
disposables.Add(DataItemAttacher.Attach(originalPath1, OperationalInterface.Dataset.ItemAs<LineList>("original path1")));
Services.DisplayObjectService.Add(originalPath1, true);
LineListDisplayObject originalpath2 = new LineListDisplayObject("planning/original path 2", Color.DarkSeaGreen);
disposables.Add(DataItemAttacher.Attach(originalpath2, OperationalInterface.Dataset.ItemAs<LineList>("original path2")));
Services.DisplayObjectService.Add(originalpath2, true);
// planned path objects
LineListDisplayObject smoothedPath = new LineListDisplayObject("planning/smoothed path", Color.OrangeRed);
disposables.Add(DataItemAttacher.Attach(smoothedPath, OperationalInterface.Dataset.ItemAs<LineList>("smoothed path")));
Services.DisplayObjectService.Add(smoothedPath, true);
LineListDisplayObject leftBound = new LineListDisplayObject("planning/left bound", Color.Blue);
disposables.Add(DataItemAttacher.Attach(leftBound, OperationalInterface.Dataset.ItemAs<LineList>("left bound")));
Services.DisplayObjectService.Add(leftBound, true);
LineListDisplayObject rightBound = new LineListDisplayObject("planning/right bound", Color.Red);
disposables.Add(DataItemAttacher.Attach(rightBound, OperationalInterface.Dataset.ItemAs<LineList>("right bound")));
Services.DisplayObjectService.Add(rightBound, true);
LineListDisplayObject subPath = new LineListDisplayObject("planning/subpath", Color.SaddleBrown);
disposables.Add(DataItemAttacher.Attach(subPath, OperationalInterface.Dataset.ItemAs<LineList>("subpath")));
Services.DisplayObjectService.Add(subPath, false);
LineListDisplayObject predictionPath = new LineListDisplayObject("planning/prediction", Color.DarkViolet);
disposables.Add(DataItemAttacher.Attach(predictionPath, OperationalInterface.Dataset.ItemAs<LineList>("prediction path")));
Services.DisplayObjectService.Add(predictionPath, false);
LineListDisplayObject predictionPath2 = new LineListDisplayObject("planning/prediction2", Color.Magenta);
disposables.Add(DataItemAttacher.Attach(predictionPath2, OperationalInterface.Dataset.ItemAs<LineList>("prediction path2")));
Services.DisplayObjectService.Add(predictionPath2, false);
LineListDisplayObject subPath2 = new LineListDisplayObject("planning/subpath2", Color.Lime);
disposables.Add(DataItemAttacher.Attach(subPath2, OperationalInterface.Dataset.ItemAs<LineList>("subpath2")));
Services.DisplayObjectService.Add(subPath2, true);
PointsDisplayObject leftPoints = new PointsDisplayObject("planning/left hits", Color.Blue, ControlPointStyle.LargeX);
disposables.Add(DataItemAttacher.Attach(leftPoints, OperationalInterface.Dataset.ItemAs<Coordinates[]>("left bound points")));
Services.DisplayObjectService.Add(leftPoints, true);
PointsDisplayObject rightPoints = new PointsDisplayObject("planning/right hits", Color.Red, ControlPointStyle.LargeX);
disposables.Add(DataItemAttacher.Attach(rightPoints, OperationalInterface.Dataset.ItemAs<Coordinates[]>("right bound points")));
Services.DisplayObjectService.Add(rightPoints, true);
LineListDisplayObject avoidancePath = new LineListDisplayObject("planning/avoidance path", Color.DarkSalmon);
disposables.Add(DataItemAttacher.Attach(avoidancePath, OperationalInterface.Dataset.ItemAs<LineList>("avoidance path")));
Services.DisplayObjectService.Add(avoidancePath, true);
// extended debugging stuff
PlanningGridExtDisplayObject gridDisplay = new PlanningGridExtDisplayObject("planning/grid", Color.DarkOrchid, Color.LightSkyBlue);
Services.DisplayObjectService.Add(gridDisplay, false);
SmoothingResultsDisplayObject smoothingDisplay = new SmoothingResultsDisplayObject("planning/smoothing");
Services.DisplayObjectService.Add(smoothingDisplay, false);
ArcDisplayObject arcs = new ArcDisplayObject();
disposables.Add(DataItemAttacher.Attach(arcs, OperationalInterface.Dataset.ItemAs<ArcVotingResults>("arc voting results")));
Services.DisplayObjectService.Add(arcs, false);
// obstacle data
untrackedClustersDisplayObject = new UntrackedClustersDisplayObject("obstacles/untracked points", true, Color.DarkMagenta);
untrackedClusterAttacher = new ChannelListenerAttacher<SceneEstimatorUntrackedClusterCollection>(untrackedClustersDisplayObject, null);
Services.DisplayObjectService.Add(untrackedClustersDisplayObject, true);
ObstacleDisplayObject perimeterObstacles = new ObstacleDisplayObject("obstacles/perimeter obstacles");
disposables.Add(DataItemAttacher.Attach(perimeterObstacles, OperationalInterface.Dataset.ItemAs<OperationalObstacle[]>("perimeter obstacles")));
Services.DisplayObjectService.Add(perimeterObstacles, true);
ObstacleDisplayObject wrappedClusters = new ObstacleDisplayObject("obstacles/wrapped obstacles");
disposables.Add(DataItemAttacher.Attach(wrappedClusters, OperationalInterface.Dataset.ItemAs<OperationalObstacle[]>("obstacles")));
Services.DisplayObjectService.Add(wrappedClusters, true);
// subscribe to the attached event so we can bind the untracked clusters data appropriately
OperationalInterface.Attached += OperationalInterface_Attached;
// lane models
LocalLaneModelDisplayObject centerLane = new LocalLaneModelDisplayObject("lanes/center", Color.DarkGreen);
disposables.Add(DataItemAttacher.Attach(centerLane, OperationalInterface.Dataset.ItemAs<LocalLaneModel>("center lane")));
Services.DisplayObjectService.Add(centerLane, false);
LocalLaneModelDisplayObject leftLane = new LocalLaneModelDisplayObject("lanes/left", Color.Goldenrod);
disposables.Add(DataItemAttacher.Attach(leftLane, OperationalInterface.Dataset.ItemAs<LocalLaneModel>("left lane")));
Services.DisplayObjectService.Add(leftLane, false);
LocalLaneModelDisplayObject rightLane = new LocalLaneModelDisplayObject("lanes/right", Color.LightCoral);
disposables.Add(DataItemAttacher.Attach(rightLane, OperationalInterface.Dataset.ItemAs<LocalLaneModel>("right lane")));
Services.DisplayObjectService.Add(rightLane, false);
LineListDisplayObject leftRoadEdge = new LineListDisplayObject("lanes/left road edge", Color.Firebrick);
disposables.Add(DataItemAttacher.Attach(leftRoadEdge, OperationalInterface.Dataset.ItemAs<LineList>("left road edge")));
Services.DisplayObjectService.Add(leftRoadEdge, true);
LineListDisplayObject rightRoadEdge = new LineListDisplayObject("lanes/right road edge", Color.DarkViolet);
disposables.Add(DataItemAttacher.Attach(rightRoadEdge, OperationalInterface.Dataset.ItemAs<LineList>("right road edge")));
Services.DisplayObjectService.Add(rightRoadEdge, true);
LineListDisplayObject roadBearing = new LineListDisplayObject("lanes/road bearing", Color.Orange);
disposables.Add(DataItemAttacher.Attach(roadBearing, OperationalInterface.Dataset.ItemAs<LineList>("road bearing")));
Services.DisplayObjectService.Add(roadBearing, true);
// intersections
LineListDisplayObject intersectionPath = new LineListDisplayObject("intersections/pull path", Color.DarkCyan);
disposables.Add(DataItemAttacher.Attach(intersectionPath, OperationalInterface.Dataset.ItemAs<LineList>("intersection path")));
Services.DisplayObjectService.Add(intersectionPath, true);
PolygonDisplayObject intersectionPolygon = new PolygonDisplayObject("intersections/polygon", Color.DarkGoldenrod, false);
disposables.Add(DataItemAttacher.Attach(intersectionPolygon, OperationalInterface.Dataset.ItemAs<Polygon>("intersection polygon")));
Services.DisplayObjectService.Add(intersectionPolygon, true);
// zones
PolygonSetDisplayObject zoneBadRegions = new PolygonSetDisplayObject("zone/bad regions", Color.DarkMagenta, false);
disposables.Add(DataItemAttacher.Attach(zoneBadRegions, OperationalInterface.Dataset.ItemAs<Polygon[]>("zone bad regions")));
Services.DisplayObjectService.Add(zoneBadRegions, true);
PolygonDisplayObject zonePerimeter = new PolygonDisplayObject("zone/perimeter", Color.DarkSeaGreen, false);
disposables.Add(DataItemAttacher.Attach(zonePerimeter, OperationalInterface.Dataset.ItemAs<Polygon>("zone perimeter")));
Services.DisplayObjectService.Add(zonePerimeter, true);
PointDisplayObject frontLeftPoint = new PointDisplayObject("zone/parker front left", "front left", Color.DarkBlue, ControlPointStyle.LargeX, ContentAlignment.MiddleRight);
disposables.Add(DataItemAttacher.Attach(frontLeftPoint, OperationalInterface.Dataset.ItemAs<Coordinates>("front left point")));
Services.DisplayObjectService.Add(frontLeftPoint, true);
PointDisplayObject rearRightPoint = new PointDisplayObject("zone/parker rear right", "rear right", Color.DarkOrchid, ControlPointStyle.LargeX, ContentAlignment.MiddleRight);
disposables.Add(DataItemAttacher.Attach(rearRightPoint, OperationalInterface.Dataset.ItemAs<Coordinates>("rear right point")));
Services.DisplayObjectService.Add(rearRightPoint, true);
CircleSegmentDisplayObject parkPath = new CircleSegmentDisplayObject("zone/parking path", Color.DarkRed);
disposables.Add(DataItemAttacher.Attach(parkPath, OperationalInterface.Dataset.ItemAs<CircleSegment>("parking path")));
Services.DisplayObjectService.Add(parkPath, true);
Services.DisplayObjectService.MoveToBefore(gridDisplay, carDisplayObject);
}
/// <summary>
/// What to do when user clicks display
/// </summary>
/// <param name="e"></param>
protected override void OnMouseDown(MouseEventArgs e)
{
#region Hit Test
// filter for vehicles or obstacles
DisplayObjectFilter dof = delegate(IDisplayObject target)
{
// check if target is network object
if (target is CarDisplayObject || target is SimObstacle)
{
return(true);
}
else
{
return(false);
}
};
// perform the hit test over the filter
HitTestResult htr = this.HitTest(transform.GetWorldPoint(new PointF(e.X, e.Y)), dof);
// check current selection if need to set as not selected
if (this.selected != null && (htr.Hit && !this.selected.Equals(htr.DisplayObject)) ||
(!htr.Hit))
{
if (this.selected != null)
{
// remove current selection
this.selected.Selected = SelectionType.NotSelected;
}
this.selected = null;
this.Simulation.simEngine.SetPropertyGridDefault();
}
#endregion
#region Left
if (e.Button == MouseButtons.Left)
{
// check if we hit a vehicle
if (htr.Hit && htr.DisplayObject is SimVehicle)
{
// display obj
CarDisplayObject cdo = (CarDisplayObject)htr.DisplayObject;
// set the vehicles as selected
cdo.Selected = SelectionType.SingleSelected;
this.selected = cdo;
this.Simulation.simEngine.propertyGrid.SelectedObject = cdo;
// check if we can move the vehicle
if (!((SimVehicle)cdo).VehicleState.IsBound)
{
// set dragging
isDragging = true;
Cursor.Current = Cursors.Hand;
// set temp
this.temporaryCoordinate = cdo.Position;
}
// redraw
this.Invalidate();
}
// check if hit obstacle
else if (htr.Hit && htr.DisplayObject is SimObstacle)
{
// set selected
this.selected = htr.DisplayObject;
this.selected.Selected = SelectionType.SingleSelected;
this.Simulation.simEngine.propertyGrid.SelectedObject = this.selected;
// check if can move
if (((SimObstacle)htr.DisplayObject).MoveAllowed)
{
// set dragging
isDragging = true;
Cursor.Current = Cursors.Hand;
// set temp
this.temporaryCoordinate = ((SimObstacle)this.selected).Position;
}
// redraw
this.Invalidate();
}
else
{
controlTag = new Point(e.X, e.Y);
isDragging = true;
Cursor.Current = Cursors.Hand;
}
}
#endregion
#region Right
else if (e.Button == MouseButtons.Right)
{
if (htr.Hit && htr.DisplayObject is CarDisplayObject)
{
this.selected = htr.DisplayObject;
this.Simulation.simEngine.propertyGrid.SelectedObject = this.selected;
this.vehicleContextMenuStrip1.Show(this, e.X, e.Y);
((CarDisplayObject)this.selected).Selected = SelectionType.SingleSelected;
}
else if (htr.Hit && htr.DisplayObject is SimObstacle)
{
// set selected
this.selected = htr.DisplayObject;
this.selected.Selected = SelectionType.SingleSelected;
this.Simulation.simEngine.propertyGrid.SelectedObject = this.selected;
// check if we can move the obstacle
if (((SimObstacle)htr.DisplayObject).MoveAllowed)
{
// set dragging
isDragging = true;
Cursor.Current = Cursors.Hand;
// set temp
this.temporaryCoordinate = transform.GetWorldPoint(new PointF(e.X, e.Y)) - ((SimObstacle)this.selected).Position;
}
// redraw
this.Invalidate();
}
}
#endregion
base.OnMouseDown(e);
this.Invalidate();
}
private void InitializeDisplayObjects()
{
grid = new GridDisplayObject();
// add seperate from display object service
Services.DisplayObjectService.Add(grid, true);
// car object
CarDisplayObject carDisplayObject = new CarDisplayObject("tahoe", Color.SkyBlue);
carDisplayObject.AttachToVehicleState = true;
Services.DisplayObjectService.Add(carDisplayObject, true);
// tracked paths
TrackedPathDisplayObject posePath = new TrackedPathDisplayObject("paths/pose", Color.LightBlue);
disposables.Add(DataItemAttacher.Attach(posePath, OperationalInterface.Dataset.ItemAs <Coordinates>("xy")));
Services.DisplayObjectService.Add(posePath, true);
TrackedPathDisplayObject posteriorPosePath = new TrackedPathDisplayObject("paths/posterior pose", Color.DarkGreen);
disposables.Add(DataItemAttacher.Attach(posteriorPosePath, OperationalInterface.Dataset.ItemAs <Coordinates>("posterior pose")));
Services.DisplayObjectService.Add(posteriorPosePath, true);
TrackedPathDisplayObject hpPath = new TrackedPathDisplayObject("paths/hp", Color.HotPink);
disposables.Add(DataItemAttacher.Attach(hpPath, OperationalInterface.Dataset.ItemAs <Coordinates>("hp xy")));
Services.DisplayObjectService.Add(hpPath, true);
TrackedPathDisplayObject sepPath = new TrackedPathDisplayObject("paths/septentrio", Color.DarkRed);
disposables.Add(DataItemAttacher.Attach(sepPath, OperationalInterface.Dataset.ItemAs <Coordinates>("gps xy")));
Services.DisplayObjectService.Add(sepPath, true);
// path point
PointDisplayObject pathPoint = new PointDisplayObject("tracking/path point", "path point", Color.Green, ControlPointStyle.LargeX, ContentAlignment.BottomRight);
disposables.Add(DataItemAttacher.Attach(pathPoint, OperationalInterface.Dataset.ItemAs <Coordinates>("path point")));
Services.DisplayObjectService.Add(pathPoint, true);
LineListDisplayObject trackingPath = new LineListDisplayObject("tracking/tracking path", Color.DeepPink);
disposables.Add(DataItemAttacher.Attach(trackingPath, OperationalInterface.Dataset.ItemAs <LineList>("tracking path")));
Services.DisplayObjectService.Add(trackingPath, false);
// u-turn stuff
CircleDisplayObject uturnCircle = new CircleDisplayObject("u-turn/planned circle", Color.Magenta);
disposables.Add(DataItemAttacher.Attach(uturnCircle, OperationalInterface.Dataset.ItemAs <Circle>("uturn circle")));
Services.DisplayObjectService.Add(uturnCircle, true);
PointDisplayObject uturnStopPoint = new PointDisplayObject("u-turn/stop point", null, Color.Black, ControlPointStyle.SmallX, ContentAlignment.BottomRight);
disposables.Add(DataItemAttacher.Attach(uturnStopPoint, OperationalInterface.Dataset.ItemAs <Coordinates>("uturn stop point")));
Services.DisplayObjectService.Add(uturnStopPoint, true);
PolygonDisplayObject uturnPolygon = new PolygonDisplayObject("u-turn/polygon", Color.BurlyWood, false);
disposables.Add(DataItemAttacher.Attach(uturnPolygon, OperationalInterface.Dataset.ItemAs <Polygon>("uturn polygon")));
Services.DisplayObjectService.Add(uturnPolygon, true);
// original path objects (paths frank sends)
LineListDisplayObject originalPath1 = new LineListDisplayObject("planning/original path 1", Color.DarkOrchid);
disposables.Add(DataItemAttacher.Attach(originalPath1, OperationalInterface.Dataset.ItemAs <LineList>("original path1")));
Services.DisplayObjectService.Add(originalPath1, true);
LineListDisplayObject originalpath2 = new LineListDisplayObject("planning/original path 2", Color.DarkSeaGreen);
disposables.Add(DataItemAttacher.Attach(originalpath2, OperationalInterface.Dataset.ItemAs <LineList>("original path2")));
Services.DisplayObjectService.Add(originalpath2, true);
// planned path objects
LineListDisplayObject smoothedPath = new LineListDisplayObject("planning/smoothed path", Color.OrangeRed);
disposables.Add(DataItemAttacher.Attach(smoothedPath, OperationalInterface.Dataset.ItemAs <LineList>("smoothed path")));
Services.DisplayObjectService.Add(smoothedPath, true);
LineListDisplayObject leftBound = new LineListDisplayObject("planning/left bound", Color.Blue);
disposables.Add(DataItemAttacher.Attach(leftBound, OperationalInterface.Dataset.ItemAs <LineList>("left bound")));
Services.DisplayObjectService.Add(leftBound, true);
LineListDisplayObject rightBound = new LineListDisplayObject("planning/right bound", Color.Red);
disposables.Add(DataItemAttacher.Attach(rightBound, OperationalInterface.Dataset.ItemAs <LineList>("right bound")));
Services.DisplayObjectService.Add(rightBound, true);
LineListDisplayObject subPath = new LineListDisplayObject("planning/subpath", Color.SaddleBrown);
disposables.Add(DataItemAttacher.Attach(subPath, OperationalInterface.Dataset.ItemAs <LineList>("subpath")));
Services.DisplayObjectService.Add(subPath, false);
LineListDisplayObject predictionPath = new LineListDisplayObject("planning/prediction", Color.DarkViolet);
disposables.Add(DataItemAttacher.Attach(predictionPath, OperationalInterface.Dataset.ItemAs <LineList>("prediction path")));
Services.DisplayObjectService.Add(predictionPath, false);
LineListDisplayObject predictionPath2 = new LineListDisplayObject("planning/prediction2", Color.Magenta);
disposables.Add(DataItemAttacher.Attach(predictionPath2, OperationalInterface.Dataset.ItemAs <LineList>("prediction path2")));
Services.DisplayObjectService.Add(predictionPath2, false);
LineListDisplayObject subPath2 = new LineListDisplayObject("planning/subpath2", Color.Lime);
disposables.Add(DataItemAttacher.Attach(subPath2, OperationalInterface.Dataset.ItemAs <LineList>("subpath2")));
Services.DisplayObjectService.Add(subPath2, true);
PointsDisplayObject leftPoints = new PointsDisplayObject("planning/left hits", Color.Blue, ControlPointStyle.LargeX);
disposables.Add(DataItemAttacher.Attach(leftPoints, OperationalInterface.Dataset.ItemAs <Coordinates[]>("left bound points")));
Services.DisplayObjectService.Add(leftPoints, true);
PointsDisplayObject rightPoints = new PointsDisplayObject("planning/right hits", Color.Red, ControlPointStyle.LargeX);
disposables.Add(DataItemAttacher.Attach(rightPoints, OperationalInterface.Dataset.ItemAs <Coordinates[]>("right bound points")));
Services.DisplayObjectService.Add(rightPoints, true);
LineListDisplayObject avoidancePath = new LineListDisplayObject("planning/avoidance path", Color.DarkSalmon);
disposables.Add(DataItemAttacher.Attach(avoidancePath, OperationalInterface.Dataset.ItemAs <LineList>("avoidance path")));
Services.DisplayObjectService.Add(avoidancePath, true);
// extended debugging stuff
PlanningGridExtDisplayObject gridDisplay = new PlanningGridExtDisplayObject("planning/grid", Color.DarkOrchid, Color.LightSkyBlue);
Services.DisplayObjectService.Add(gridDisplay, false);
SmoothingResultsDisplayObject smoothingDisplay = new SmoothingResultsDisplayObject("planning/smoothing");
Services.DisplayObjectService.Add(smoothingDisplay, false);
ArcDisplayObject arcs = new ArcDisplayObject();
disposables.Add(DataItemAttacher.Attach(arcs, OperationalInterface.Dataset.ItemAs <ArcVotingResults>("arc voting results")));
Services.DisplayObjectService.Add(arcs, false);
// obstacle data
untrackedClustersDisplayObject = new UntrackedClustersDisplayObject("obstacles/untracked points", true, Color.DarkMagenta);
untrackedClusterAttacher = new ChannelListenerAttacher <SceneEstimatorUntrackedClusterCollection>(untrackedClustersDisplayObject, null);
Services.DisplayObjectService.Add(untrackedClustersDisplayObject, true);
ObstacleDisplayObject perimeterObstacles = new ObstacleDisplayObject("obstacles/perimeter obstacles");
disposables.Add(DataItemAttacher.Attach(perimeterObstacles, OperationalInterface.Dataset.ItemAs <OperationalObstacle[]>("perimeter obstacles")));
Services.DisplayObjectService.Add(perimeterObstacles, true);
ObstacleDisplayObject wrappedClusters = new ObstacleDisplayObject("obstacles/wrapped obstacles");
disposables.Add(DataItemAttacher.Attach(wrappedClusters, OperationalInterface.Dataset.ItemAs <OperationalObstacle[]>("obstacles")));
Services.DisplayObjectService.Add(wrappedClusters, true);
// subscribe to the attached event so we can bind the untracked clusters data appropriately
OperationalInterface.Attached += OperationalInterface_Attached;
// lane models
LocalLaneModelDisplayObject centerLane = new LocalLaneModelDisplayObject("lanes/center", Color.DarkGreen);
disposables.Add(DataItemAttacher.Attach(centerLane, OperationalInterface.Dataset.ItemAs <LocalLaneModel>("center lane")));
Services.DisplayObjectService.Add(centerLane, false);
LocalLaneModelDisplayObject leftLane = new LocalLaneModelDisplayObject("lanes/left", Color.Goldenrod);
disposables.Add(DataItemAttacher.Attach(leftLane, OperationalInterface.Dataset.ItemAs <LocalLaneModel>("left lane")));
Services.DisplayObjectService.Add(leftLane, false);
LocalLaneModelDisplayObject rightLane = new LocalLaneModelDisplayObject("lanes/right", Color.LightCoral);
disposables.Add(DataItemAttacher.Attach(rightLane, OperationalInterface.Dataset.ItemAs <LocalLaneModel>("right lane")));
Services.DisplayObjectService.Add(rightLane, false);
LineListDisplayObject leftRoadEdge = new LineListDisplayObject("lanes/left road edge", Color.Firebrick);
disposables.Add(DataItemAttacher.Attach(leftRoadEdge, OperationalInterface.Dataset.ItemAs <LineList>("left road edge")));
Services.DisplayObjectService.Add(leftRoadEdge, true);
LineListDisplayObject rightRoadEdge = new LineListDisplayObject("lanes/right road edge", Color.DarkViolet);
disposables.Add(DataItemAttacher.Attach(rightRoadEdge, OperationalInterface.Dataset.ItemAs <LineList>("right road edge")));
Services.DisplayObjectService.Add(rightRoadEdge, true);
LineListDisplayObject roadBearing = new LineListDisplayObject("lanes/road bearing", Color.Orange);
disposables.Add(DataItemAttacher.Attach(roadBearing, OperationalInterface.Dataset.ItemAs <LineList>("road bearing")));
Services.DisplayObjectService.Add(roadBearing, true);
// intersections
LineListDisplayObject intersectionPath = new LineListDisplayObject("intersections/pull path", Color.DarkCyan);
disposables.Add(DataItemAttacher.Attach(intersectionPath, OperationalInterface.Dataset.ItemAs <LineList>("intersection path")));
Services.DisplayObjectService.Add(intersectionPath, true);
PolygonDisplayObject intersectionPolygon = new PolygonDisplayObject("intersections/polygon", Color.DarkGoldenrod, false);
disposables.Add(DataItemAttacher.Attach(intersectionPolygon, OperationalInterface.Dataset.ItemAs <Polygon>("intersection polygon")));
Services.DisplayObjectService.Add(intersectionPolygon, true);
// zones
PolygonSetDisplayObject zoneBadRegions = new PolygonSetDisplayObject("zone/bad regions", Color.DarkMagenta, false);
disposables.Add(DataItemAttacher.Attach(zoneBadRegions, OperationalInterface.Dataset.ItemAs <Polygon[]>("zone bad regions")));
Services.DisplayObjectService.Add(zoneBadRegions, true);
PolygonDisplayObject zonePerimeter = new PolygonDisplayObject("zone/perimeter", Color.DarkSeaGreen, false);
disposables.Add(DataItemAttacher.Attach(zonePerimeter, OperationalInterface.Dataset.ItemAs <Polygon>("zone perimeter")));
Services.DisplayObjectService.Add(zonePerimeter, true);
PointDisplayObject frontLeftPoint = new PointDisplayObject("zone/parker front left", "front left", Color.DarkBlue, ControlPointStyle.LargeX, ContentAlignment.MiddleRight);
disposables.Add(DataItemAttacher.Attach(frontLeftPoint, OperationalInterface.Dataset.ItemAs <Coordinates>("front left point")));
Services.DisplayObjectService.Add(frontLeftPoint, true);
PointDisplayObject rearRightPoint = new PointDisplayObject("zone/parker rear right", "rear right", Color.DarkOrchid, ControlPointStyle.LargeX, ContentAlignment.MiddleRight);
disposables.Add(DataItemAttacher.Attach(rearRightPoint, OperationalInterface.Dataset.ItemAs <Coordinates>("rear right point")));
Services.DisplayObjectService.Add(rearRightPoint, true);
CircleSegmentDisplayObject parkPath = new CircleSegmentDisplayObject("zone/parking path", Color.DarkRed);
disposables.Add(DataItemAttacher.Attach(parkPath, OperationalInterface.Dataset.ItemAs <CircleSegment>("parking path")));
Services.DisplayObjectService.Add(parkPath, true);
Services.DisplayObjectService.MoveToBefore(gridDisplay, carDisplayObject);
}
