Beispiel #1
0
        /// <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);
            }
        }
Beispiel #2
0
        /// <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));
        }