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