/// <summary>
/// Constructor
/// Defines a link between a specific RndfWaypoint and Total Time until the next Goal.
/// COntains specific information about actions at the action RndfWaypoint
/// </summary>
/// <param name="actionPoint">Waypoint at which vehicle exits the Segment or alternatively where vehicle needs to pass over goal</param>
/// <param name="timeToAction">Time to get to action point</param>
/// <param name="routeTime">Total route time to goal if go through this action point</param>
/// <param name="continuation">If we decide to continue on road after action point, this is route time to goal</param>
public LocalRoute(RndfWaypointID actionPoint, double timeToAction, double routeTime, LocalOption continuation)
{
this.actionPoint = actionPoint;
this.timeToAction = timeToAction;
this.routeTime = routeTime;
this.continuation = continuation;
}
/// <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>
/// 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="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;
}
/// <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");
}
private LocalOption continuation; // If we decide to continue on road after action point, this is route time to goal
/// <summary>
/// Constructor
/// Defines a link between a specific RndfWaypoint and Total Time until the next Goal.
/// COntains specific information about actions at the action RndfWaypoint
/// </summary>
/// <param name="actionPoint">Waypoint at which vehicle exits the Segment or alternatively where vehicle needs to pass over goal</param>
/// <param name="timeToAction">Time to get to action point</param>
public LocalRoute(RndfWaypointID actionPoint, double timeToAction)
{
this.actionPoint = actionPoint;
this.timeToAction = timeToAction;
}
public GoalsDisplay(RndfNetwork rndf, RndfWaypointID current, Queue <RndfWaypointID> remaining)
{
this.rndf = rndf;
this.Current = current;
this.GoalsRemaining = remaining;
}
/// <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="goalPoint">Waypoint at which vehicle needs to pass over goal</param>
/// <param name="timeToAction">Time to get to goal</param>
/// <param name="routeTime">Total route time to goal if go through this action point</param>
/// <param name="continuation">If we decide to continue on road after action point, this is route time to goal</param>
/// <param name="goal">goal the rndf waypoint represents</param>
public GoalLocalRoute(RndfWaypointID goalPoint, double timeToAction, double routeTime, LocalOption continuation, Goal goal)
: base(goalPoint, timeToAction, routeTime, continuation)
{
this.goal = goal;
}
/// <summary>
/// Constructor
/// Defines a link between a specific RndfWaypoint and Total Time until the next Goal.
/// Contains specific information about actions at the action RndfWaypoint
/// </summary>
/// <param name="actionPoint">Waypoint at which vehicle exits the Segment or alternatively where vehicle needs to pass over goal</param>
/// <param name="timeToAction">Time to get to action point</param>
public GoalLocalRoute(RndfWaypointID actionPoint, double timeToAction, Goal goal)
: base(actionPoint, timeToAction)
{
this.goal = goal;
this.RouteTime = timeToAction;
}
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");
}
/// <summary>
/// Constructor
/// Defines a link between a specific RndfWaypoint and Total Time until the next Goal.
/// COntains specific information about actions at the action RndfWaypoint
/// </summary>
/// <param name="actionPoint">Waypoint at which vehicle exits the Segment or alternatively where vehicle needs to pass over goal</param>
/// <param name="timeToAction">Time to get to action point</param>
/// <param name="routeTime">Total route time to goal if go through this action point</param>
/// <param name="continuation">If we decide to continue on road after action point, this is route time to goal</param>
public ExitLocalRoute(RndfWaypointID actionPoint, double timeToAction, double routeTime, LocalOption continuation, List <RndfWayPoint> nodes)
: base(actionPoint, timeToAction, routeTime, continuation)
{
this.exits = new List <LocalOption>();
this.nodes = nodes;
}
public StopLineLaneSpeedCommand(double expectedDistance, double maxSpeed, double searchSpeed, RndfWaypointID waypointID)
: base(expectedDistance, maxSpeed)
{
this.searchSpeed = searchSpeed;
this.waypointID = waypointID;
}
public TurnBehavior(RndfWaypointID exitPoint, RndfWaypointID entryPoint, double maxSpeed)
{
this.exitPoint = exitPoint;
this.entryPoint = entryPoint;
this.maxSpeed = maxSpeed;
}
