/// <summary> /// Navigation parameterization given point of intereset and our vehicle state /// </summary> /// <param name="vehicleState"></param> /// <param name="dpoi"></param> /// <returns></returns> private TravelingParameters NavigationParameterization(VehicleState vehicleState, DownstreamPointOfInterest dpoi, ArbiterWaypoint final, LinePath fullPath) { // stop waypoint and distance ArbiterWaypoint stopWaypoint = dpoi.PointOfInterest; double distanceToStop = final.Lane.DistanceBetween(final.Position, stopWaypoint.Position) + vehicleState.Front.DistanceTo(this.turn.FinalGeneric.Position); // checks if we need to stop at this point bool isStop = dpoi.IsExit || (dpoi.IsGoal && dpoi.PointOfInterest.IsCheckpoint && dpoi.PointOfInterest.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber) && CoreCommon.Mission.MissionCheckpoints.Count == 1); // get next lane type stop List <WaypointType> stopTypes = new List <WaypointType>(); stopTypes.Add(WaypointType.End); stopTypes.Add(WaypointType.Stop); ArbiterWaypoint nextStop = final.Lane.GetNext(final, stopTypes, new List <ArbiterWaypoint>()); // get total distance to lane stop double distanceToLaneStop = final.Lane.DistanceBetween(final.Position, nextStop.Position) + vehicleState.Front.DistanceTo(this.turn.FinalGeneric.Position); // check that we have nearest and correct stop if (!isStop || (isStop && distanceToLaneStop <= distanceToStop)) { stopWaypoint = nextStop; distanceToStop = distanceToLaneStop; } // get speed to stop double stopSpeed = this.StoppingParameters(distanceToStop, final.Lane.Way.Segment.SpeedLimits.MaximumSpeed); // get params TravelingParameters navParams = this.GetParameters(stopSpeed, distanceToStop, final, vehicleState, true); // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = navParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = navParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = navParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = navParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = navParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = navParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = navParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = "Dpoi: " + dpoi.PointOfInterest.Equals(stopWaypoint) + ", Stop: " + isStop.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = stopWaypoint.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = Math.Min(final.Lane.CurrentMaximumSpeed(vehicleState.Front), this.turn.MaximumDefaultSpeed).ToString("F2"); // return return(navParams); }
private void ShiftNetworkOkButton_Click(object sender, EventArgs e) { try { double east = double.Parse(this.ShiftNetworkEastTextBox.Text); double north = double.Parse(this.ShiftNetworkNorthTextBox.Text); Coordinates shift = new Coordinates(east, north); foreach (IArbiterWaypoint iaw in roads.ArbiterWaypoints.Values) { iaw.Position = iaw.Position + shift; } // safety zone filter DisplayObjectFilter f = delegate(IDisplayObject target) { // check if target is network object if (target is ArbiterSafetyZone) { return(true); } else { return(false); } }; // remove safety zones display.RemoveDisplayObjectType(f); // new safety roads.ArbiterSafetyZones = new List <ArbiterSafetyZone>(); // remove from network List <IDisplayObject> displayObjects = new List <IDisplayObject>(); foreach (IDisplayObject ido in roads.DisplayObjects) { if (!f(ido)) { displayObjects.Add(ido); } } // remove lane safety zones, create new partition paths foreach (ArbiterSegment asg in roads.ArbiterSegments.Values) { foreach (ArbiterLane al in asg.Lanes.Values) { al.SafetyZones = new List <ArbiterSafetyZone>(); // path segments of lane List <IPathSegment> pathSegments = new List <IPathSegment>(); // loop foreach (ArbiterLanePartition alPar in al.Partitions) { // make new segment pathSegments.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position)); } // generate lane partition path Path partitionPath = new Path(pathSegments); al.PartitionPath = partitionPath; } } // recreate safety zones foreach (IArbiterWaypoint iaw in roads.ArbiterWaypoints.Values) { if (iaw is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)iaw; if (aw.IsStop) { ArbiterLane al = aw.Lane; LinePath.PointOnPath end = al.GetClosestPoint(aw.Position); double dist = -30; LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist); if (dist != 0) { EditorOutput.WriteLine(aw.ToString() + " safety zone too close to start of lane, setting start to start of lane"); begin = al.LanePath().StartPoint; } ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin); asz.isExit = true; asz.Exit = aw; al.SafetyZones.Add(asz); roads.DisplayObjects.Add(asz); roads.ArbiterSafetyZones.Add(asz); display.AddDisplayObject(asz); } } } // redraw this.display.Invalidate(); // notify EditorOutput.WriteLine("Shifted road network: east: " + east.ToString("F6") + ", north: " + north.ToString("F6")); EditorOutput.WriteLine("Recomputed position-dependent types"); // close form this.Close(); } catch (Exception ex) { EditorOutput.WriteLine("Shift of road network failed: \n" + ex.ToString()); } }
public string LongDescription() { return("Stopping At Exit: " + waypoint.ToString()); }
public string StateInformation() { return(waypoint.ToString()); }