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