/// <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);
}
}
/// <summary>
/// Generates a path around a turn
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
/// <param name="turn"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleRelative"></param>
/// <returns></returns>
public static IPath TurnPath(Lane initial, Lane final, Interconnect turn, VehicleState vehicleState, bool vehicleRelative)
{
// 1. list of absolute positions of lane coordinates
List <Coordinates> absoluteCoordinates = new List <Coordinates>();
// make sure not turning onto same lane
if (!initial.LaneID.Equals(final.LaneID))
{
// 2. loop through initial lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true));
// 3. check if the turn has user partitions
if (turn.UserPartitions != null)
{
// 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation
foreach (UserPartition partition in turn.UserPartitions)
{
// add if not an rndf waypoint
if (!(partition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(partition.InitialWaypoint.Position);
}
}
}
// 4. add the next lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true));
}
else
{
// get lanegth of lane / 4
// HACK
double length = RoadToolkit.LanePath(initial, vehicleState, true).Length / 4;
// 2. loop through initial lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true, length, false));
// 3. check if the turn has user partitions
if (turn.UserPartitions != null)
{
// 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation
foreach (UserPartition partition in turn.UserPartitions)
{
// add if not an rndf waypoint
if (!(partition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(partition.InitialWaypoint.Position);
}
}
}
// 4. loop through final lane's waypoints
absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true, length, true));
}
// 5. Generate path
return(GeneratePathFromCoordinates(absoluteCoordinates, vehicleState.Position, vehicleState.Heading, vehicleRelative));
}