/// <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>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="stopType"></param>
public StoppingState(LaneID lane, RndfWaypointID waypoint, StopType stopType, LaneDescription currentLane)
: base(lane, lane)
{
this.stopType = stopType;
this.waypoint = waypoint;
this.currentLane = currentLane;
}
/// <summary>
/// Checks if two RndfWaypointID's are equal
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
if (obj is RndfWaypointID)
{
RndfWaypointID wid = (RndfWaypointID)obj;
return(wid.waypointNumber == waypointNumber && wid.laneID.Equals(laneID));
}
else
{
return(false);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="currentArbiterState"></param>
/// <param name="internalCarMode"></param>
/// <param name="fullRoute"></param>
/// <param name="routeTime"></param>
/// <param name="actionPoint"></param>
/// <param name="currentGoal"></param>
/// <param name="goals"></param>
/// <param name="behavior"></param>
public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime,
RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue<RndfWaypointID> goals, Behavior behavior, VehicleState planningState)
{
this.CurrentArbiterState = currentArbiterState;
this.InternalCarMode = internalCarMode;
this.FullRoute = fullRoute;
this.RouteTime = routeTime;
this.ActionPoint = actionPoint;
this.CurrentGoal = currentGoal;
this.Goals = goals;
this.Behavior = behavior;
this.PlanningState = planningState;
}
public TurnBehavior(RndfWaypointID exitPoint, RndfWaypointID entryPoint, double maxSpeed)
{
this.exitPoint = exitPoint;
this.entryPoint = entryPoint;
this.maxSpeed = maxSpeed;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public InterconnectID(RndfWaypointID initial, RndfWaypointID final)
{
this.initialID = initial;
this.finalID = final;
}
/// <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");
}
public override void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound)
{
if(behavior is UTurnBehavior)
{
//
}
// check to see if we are at the upper bound
RndfWayPoint upperWaypoint = channelListener.RndfNetwork.Waypoints[upperBound];
if (!(behavior is TurnBehavior) && upperWaypoint.NextLanePartition != null && location.DistanceTo(upperWaypoint.Position) < 3)
{
lowerBound = upperWaypoint.WaypointID;
upperBound = upperWaypoint.NextLanePartition.FinalWaypoint.WaypointID;
}
Console.WriteLine(" > Received Instruction to Execute Behavior: " + behavior.ToString());
if (behavior is StayInLaneBehavior)
{
StayInLaneBehavior stayInLane = (StayInLaneBehavior)behavior;
if (stayInLane.SpeedCommand is StopLineLaneSpeedCommand)
{
StopLineLaneSpeedCommand speedCommand = (StopLineLaneSpeedCommand)stayInLane.SpeedCommand;
if (speedCommand.Distance < 2)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = location;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 0;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
///Console.WriteLine(" > Published Position");
}
else
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
///Console.WriteLine(" > Published Position");
}
}
else if (stayInLane.SpeedCommand is StopLaneSpeedCommand)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = location;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = -5;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
///Console.WriteLine(" > Published Position");
}
else if(stayInLane.SpeedCommand is DefaultLaneSpeedCommand)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
//Console.WriteLine(" > Published Position");
}
else
{
throw new ArgumentException("Unknown Lane Speed Type", "stayInLane.SpeedCommand");
}
}
// TODO: include midway point
else if (behavior is TurnBehavior)
{
TurnBehavior currentBehavior = (TurnBehavior)behavior;
RndfWayPoint exitWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.ExitPoint];
if (location.DistanceTo(exitWaypoint.Position) < 0.1)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
RndfWayPoint entryWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint];
Common.Coordinates change = entryWaypoint.Position - exitWaypoint.Position;
Common.Coordinates midpoint = exitWaypoint.Position + change/2;
LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.ExitPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1);
vehicleState.xyPosition = midpoint;
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
}
else
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint].Position;
LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.EntryPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
//Console.WriteLine(" > Published Position");
}
}
else
{
throw new ArgumentException("Unknown Behavior Type", "behavior");
}
//Console.WriteLine("Sent Back Position \n");
}
public GoalsDisplay(RndfNetwork rndf, RndfWaypointID current, Queue<RndfWaypointID> remaining)
{
this.rndf = rndf;
this.Current = current;
this.GoalsRemaining = remaining;
}
public StopLineLaneSpeedCommand(double expectedDistance, double maxSpeed, double searchSpeed, RndfWaypointID waypointID)
: base(expectedDistance, maxSpeed)
{
this.searchSpeed = searchSpeed;
this.waypointID = waypointID;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypointID">RndfWaypoint Identification Information</param>
/// <param name="lane">Lane this wypoint belongs to</param>
public RndfWayPoint(RndfWaypointID waypointID, Lane lane)
{
this.waypointID = waypointID;
this.lane = lane;
}
/// <summary> /// Execute the Behavior /// </summary> /// <param name="behavior"></param> public abstract void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound);
/// <summary>
/// Constructor
/// </summary>
/// <param name="goalNumber">id number of goal in checkpoint</param>
/// <param name="waypointID">rndf waypoint this checkpoint identifies</param>
public Goal(int goalNumber, RndfWaypointID waypointID)
{
this.goalNumber = goalNumber;
this.waypointID = waypointID;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypointID">RndfWaypoint Identification Information</param>
/// <param name="lane">Lane this wypoint belongs to</param>
public RndfWayPoint(RndfWaypointID waypointID, Lane lane)
{
this.waypointID = waypointID;
this.lane = lane;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="final"></param>
public InterconnectID(RndfWaypointID initial, RndfWaypointID final)
{
this.initialID = initial;
this.finalID = final;
}
