/// <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> /// Generates a path around a turn /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <param name="turn"></param> /// <param name="vehicleState"></param> /// <param name="vehicleRelative"></param> /// <returns></returns> public static IPath TurnPath(Lane initial, Lane final, Interconnect turn, VehicleState vehicleState, bool vehicleRelative) { // 1. list of absolute positions of lane coordinates List <Coordinates> absoluteCoordinates = new List <Coordinates>(); // make sure not turning onto same lane if (!initial.LaneID.Equals(final.LaneID)) { // 2. loop through initial lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true)); // 3. check if the turn has user partitions if (turn.UserPartitions != null) { // 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation foreach (UserPartition partition in turn.UserPartitions) { // add if not an rndf waypoint if (!(partition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position absoluteCoordinates.Add(partition.InitialWaypoint.Position); } } } // 4. add the next lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true)); } else { // get lanegth of lane / 4 // HACK double length = RoadToolkit.LanePath(initial, vehicleState, true).Length / 4; // 2. loop through initial lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true, length, false)); // 3. check if the turn has user partitions if (turn.UserPartitions != null) { // 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation foreach (UserPartition partition in turn.UserPartitions) { // add if not an rndf waypoint if (!(partition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position absoluteCoordinates.Add(partition.InitialWaypoint.Position); } } } // 4. loop through final lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true, length, true)); } // 5. Generate path return(GeneratePathFromCoordinates(absoluteCoordinates, vehicleState.Position, vehicleState.Heading, vehicleRelative)); }