/// <summary>
/// Gets a default behavior for a path segment
/// </summary>
/// <param name="location"></param>
/// <param name="vehicleState"></param>
/// <param name="exit"></param>
/// <param name="relative"></param>
/// <param name="stopSpeed"></param>
/// <param name="aMax"></param>
/// <param name="dt">timestep in seconds</param>
/// <returns></returns>
public static PathFollowingBehavior DefaultStayInLaneBehavior(RndfLocation location, VehicleState vehicleState,
RndfWaypointID action, ActionType actionType, bool relative, double stopSpeed, double aMax, double dt,
double maxSpeed, IPath path)
{
// get lane path
//IPath path = RoadToolkit.LanePath(location.Partition.FinalWaypoint.Lane, vehicleState, relative);
// check if the action is just a goal (note that exit and stop take precedence)
if (actionType == ActionType.Goal)
{
// get maximum speed
//double maxSpeed = location.Partition.FinalWaypoint.Lane.Way.Segment.SpeedInformation.MaxSpeed;
//double maxSpeed = maxV;
// generate path following behavior
//return new PathFollowingBehavior(path, new ScalarSpeedCommand(maxSpeed));
return null;
}
else
{
// get maximum speed
//double maxSpeed = location.Partition.FinalWaypoint.Lane.Way.Segment.SpeedInformation.MaxSpeed;
// get operational required distance to hand over to operational stop
double distance = RoadToolkit.DistanceUntilOperationalStop(RoadToolkit.DistanceToWaypoint(location, action)-TahoeParams.FL);
// get desired velocity
double desiredSpeed = RoadToolkit.InferFinalSpeed(0, stopSpeed, distance, maxSpeed, aMax, dt);
// generate path following behavior
//return new PathFollowingBehavior(path, new ScalarSpeedCommand(desiredSpeed));
return null;
}
}
public static void NextStop(RndfNetwork rndf, RndfLocation vehicleLocation,
out RndfWayPoint nextStop, out double distance)
{
// set current as final waypoint on current partition
RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint;
// set initial distance
distance = vehicleLocation.AbsolutePositionOnPartition.DistanceTo(current.Position);
// iterate over waypoints
while (!current.IsStop && current.NextLanePartition != null)
{
// update distance
distance += current.Position.DistanceTo(current.NextLanePartition.FinalWaypoint.Position);
// update current
current = current.NextLanePartition.FinalWaypoint;
}
if (current.IsStop)
{
nextStop = current;
// return current as stop or end of lane
return;
}
else
{
nextStop = null;
return;
}
}
/// <summary>
/// Gets the next point at which we must stop
/// </summary>
/// <param name="rndf"></param>
/// <param name="vehicleLocation"></param>
/// <returns></returns>
public static RndfWayPoint NextStopOrEnd(RndfNetwork rndf, RndfLocation vehicleLocation)
{
// set current as final waypoint on current partition
RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint;
// iterate over waypoints
while (!current.IsStop && current.NextLanePartition != null)
{
// update current
current = current.NextLanePartition.FinalWaypoint;
}
// return current as stop or end of lane
return current;
}
/// <summary>
/// Gets the distance to a forward waypoint
/// </summary>
/// <param name="location"></param>
/// <param name="waypoint"></param>
/// <returns></returns>
public static double DistanceToWaypoint(RndfLocation location, RndfWaypointID waypoint)
{
double distance = location.AbsolutePositionOnPartition.DistanceTo(location.Partition.FinalWaypoint.Position);
RndfWayPoint current = location.Partition.FinalWaypoint;
while (current != null)
{
if (current.WaypointID.Equals(waypoint))
{
return distance;
}
else if (current.NextLanePartition == null)
{
throw new Exception("waypoint not ahead of vehicle on current lane");
}
else
{
distance += current.Position.DistanceTo(current.NextLanePartition.FinalWaypoint.Position);
current = current.NextLanePartition.FinalWaypoint;
}
}
throw new Exception("waypoint not ahead of vehicle on current lane");
}
