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