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