public static void NextStop(RndfNetwork rndf, RndfLocation vehicleLocation, out RndfWayPoint nextStop, out double distance) { // set current as final waypoint on current partition RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint; // set initial distance distance = vehicleLocation.AbsolutePositionOnPartition.DistanceTo(current.Position); // iterate over waypoints while (!current.IsStop && current.NextLanePartition != null) { // update distance distance += current.Position.DistanceTo(current.NextLanePartition.FinalWaypoint.Position); // update current current = current.NextLanePartition.FinalWaypoint; } if (current.IsStop) { nextStop = current; // return current as stop or end of lane return; } else { nextStop = null; return; } }
/// <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> /// Gets the next point at which we must stop /// </summary> /// <param name="rndf"></param> /// <param name="vehicleLocation"></param> /// <returns></returns> public static RndfWayPoint NextStopOrEnd(RndfNetwork rndf, RndfLocation vehicleLocation) { // set current as final waypoint on current partition RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint; // iterate over waypoints while (!current.IsStop && current.NextLanePartition != null) { // update current current = current.NextLanePartition.FinalWaypoint; } // return current as stop or end of lane return(current); }
/// <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"); }
/// <summary> /// Constructor /// </summary> /// <param name="observed"></param> /// <param name="vehicleLocation"></param> public VehicleInformation(ObservedVehicle observed, RndfLocation vehicleLocation) { this.Observed = observed; this.VehicleLocation = vehicleLocation; }