Example #1
0
        public string PartitionIdString()
        {
            string final = "";

            #region Standard Areas

            List <AreaProbability> AreaProbabilities = new List <AreaProbability>();

            Circle  circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
            Polygon conv = circ.ToPolygon(32);

            Circle  circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
            Polygon conv1 = circ1.ToPolygon(32);

            Circle  circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
            Polygon conv2 = circ2.ToPolygon(32);

            for (int i = 0; i < this.trackedCluster.closestPartitions.Length; i++)
            {
                SceneEstimatorClusterPartition secp = this.trackedCluster.closestPartitions[i];

                if (RemoraCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
                {
                    IVehicleArea iva = RemoraCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];

                    bool found = false;
                    for (int j = 0; j < AreaProbabilities.Count; j++)
                    {
                        AreaProbability ap = AreaProbabilities[j];

                        if (ap.Key.Equals(iva))
                        {
                            ap.Value = ap.Value + secp.probability;
                            found    = true;
                        }
                    }

                    if (!found)
                    {
                        AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
                    }
                }
                else
                {
                    RemoraOutput.WriteLine("Remora caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map", OutputType.Remora);
                }
            }

            if (AreaProbabilities.Count > 0)
            {
                double rP = 0.0;
                foreach (AreaProbability ap in AreaProbabilities)
                {
                    if (ap.Key is ArbiterLane)
                    {
                        rP += ap.Value;
                    }
                }

                if (rP > 0.1)
                {
                    foreach (AreaProbability ap in AreaProbabilities)
                    {
                        if (ap.Key is ArbiterLane)
                        {
                            // proabbility says in area
                            double vP = ap.Value / rP;
                            if (vP > 0.3)
                            {
                                #region Check if obstacle enough in area

                                bool ok = false;
                                if (ap.Key is ArbiterLane)
                                {
                                    VehicleState vs = RemoraCommon.Communicator.GetVehicleState();

                                    ArbiterLane al      = (ArbiterLane)ap.Key;
                                    Coordinates closest = this.ClosestPointToLine(al.LanePath(), vs).Value;

                                    // dist to closest
                                    double distanceToClosest = vs.Front.DistanceTo(closest);

                                    // get our dist to closest
                                    if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(RemoraCommon.Communicator.GetVehicleSpeed().Value))))
                                    {
                                        if (al.LanePolygon != null)
                                        {
                                            ok = this.VehicleExistsInsidePolygon(al.LanePolygon, vs);
                                        }
                                        else
                                        {
                                            ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
                                        }
                                    }
                                    else if (distanceToClosest <= 30.0)
                                    {
                                        if (al.LanePolygon != null)
                                        {
                                            if (!this.trackedCluster.isStopped && this.VehicleAllInsidePolygon(al.LanePolygon, vs))
                                            {
                                                ok = true;
                                            }
                                            else
                                            {
                                                if (this.trackedCluster.isStopped)
                                                {
                                                    bool isInSafety = false;
                                                    foreach (ArbiterIntersection ai in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
                                                    {
                                                        if (ai.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint))
                                                        {
                                                            isInSafety = true;
                                                        }
                                                    }
                                                    foreach (ArbiterSafetyZone asz in al.SafetyZones)
                                                    {
                                                        if (asz.IsInSafety(this.trackedCluster.closestPoint))
                                                        {
                                                            isInSafety = true;
                                                        }
                                                    }

                                                    if (isInSafety)
                                                    {
                                                        if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv1))
                                                        {
                                                            ok = true;
                                                        }
                                                    }
                                                    else
                                                    {
                                                        if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv))
                                                        {
                                                            ok = true;
                                                        }
                                                    }
                                                }
                                                else
                                                {
                                                    if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv2))
                                                    {
                                                        ok = true;
                                                    }
                                                }
                                            }
                                        }
                                        else
                                        {
                                            ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
                                        }
                                    }
                                    else
                                    {
                                        ok = true;
                                    }

                                    if (ok)
                                    {
                                        final = final + ap.Key.ToString() + ": " + vP.ToString("F4") + "\n";
                                    }
                                }

                                #endregion
                            }
                        }
                    }
                }
            }

            #endregion

            #region Interconnect Area Mappings

            foreach (ArbiterInterconnect ai in RemoraCommon.RoadNetwork.ArbiterInterconnects.Values)
            {
                if (ai.TurnPolygon.IsInside(this.trackedCluster.closestPoint))
                {
                    final = final + ai.ToString() + "\n";

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn && ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
                    {
                        // initial
                        ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
                        ArbiterLane targetLane  = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
                        final = final + "UTurn2Lane: " + initialLane.ToString() + " & " + targetLane.ToString() + "\n";
                    }
                }
            }

            #endregion

            #region Intersections

            foreach (ArbiterIntersection aInt in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
            {
                if (aInt.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint))
                {
                    final = final + "Inter: " + aInt.ToString() + "\n";
                }
            }

            #endregion

            return(final);
        }
 public string StateInformation()
 {
     return(TargetLane.ToString());
 }
 public string LongDescription()
 {
     return("Staying in lane: " + Lane.ToString());
 }
        /// <summary>
        /// Startup the vehicle from a certain position, pass the next state back,
        /// and initialize the lane agent if possible
        /// </summary>
        /// <param name="vehicleState"></param>
        /// <returns></returns>
        public IState Startup(VehicleState vehicleState, CarMode carMode)
        {
            // check car mode
            if (carMode == CarMode.Run)
            {
                // check areas
                ArbiterLane         al  = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
                ArbiterZone         az  = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
                ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
                ArbiterInterconnect ai  = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);

                if (az != null)
                {
                    // special zone startup
                    return(new ZoneStartupState(az));
                }

                if (ain != null)
                {
                    if (al != null)
                    {
                        // check lane stuff
                        PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);

                        // get distance from front of car
                        double dist = lanePoint.pt.DistanceTo(vehicleState.Front);

                        // check dist to lane
                        if (dist < al.Width + 1.0)
                        {
                            // check orientation relative to lane
                            Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();

                            // get our heading
                            Coordinates ourHeading = vehicleState.Heading.Normalize();

                            // forwards or backwards
                            bool forwards = true;

                            // check dist to each other
                            if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
                            {
                                // not going forwards
                                forwards = false;
                            }

                            // stay in lane if forwards
                            if (forwards)
                            {
                                ArbiterOutput.Output("Starting up in lane: " + al.ToString());
                                return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
                            }
                            else
                            {
                                // Opposing lane
                                return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
                            }
                        }
                    }

                    // startup intersection state
                    return(new IntersectionStartupState(ain));
                }

                if (al != null)
                {
                    // get a startup chute
                    ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);

                    // check if in a startup chute
                    if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
                    {
                        ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
                        return(new StartupOffChuteState(startupChute));
                    }
                    // not in a startup chute
                    else
                    {
                        PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);

                        // get distance from front of car
                        double dist = lanePoint.pt.DistanceTo(vehicleState.Front);

                        // check orientation relative to lane
                        Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();

                        // get our heading
                        Coordinates ourHeading = vehicleState.Heading.Normalize();

                        // forwards or backwards
                        bool forwards = true;

                        // check dist to each other
                        if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
                        {
                            // not going forwards
                            forwards = false;
                        }

                        // stay in lane if forwards
                        if (forwards)
                        {
                            ArbiterOutput.Output("Starting up in lane: " + al.ToString());
                            return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
                        }
                        else
                        {
                            // opposing
                            return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
                        }
                    }
                }

                // fell out
                ArbiterOutput.Output("Cannot find area to startup in");
                return(CoreCommon.CorePlanningState);
            }
            else
            {
                return(CoreCommon.CorePlanningState);
            }
        }
Example #5
0
        /// <summary>
        /// Check if we can go
        /// </summary>
        /// <param name="vs"></param>
        private bool CanGo(VehicleState vs)
        {
            #region Moving Vehicles Inside Turn

            // check if we can still go through this turn
            if (TacticalDirector.VehicleAreas.ContainsKey(this.turn))
            {
                // get the subpath of the interconnect we care about
                LinePath.PointOnPath frontPos  = this.turn.InterconnectPath.GetClosestPoint(vs.Front);
                LinePath             aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint);

                if (aiSubpath.PathLength > 4.0)
                {
                    aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0);

                    // get vehicles
                    List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn];

                    // loop vehicles
                    foreach (VehicleAgent va in turnVehicles)
                    {
                        // check if inside turn
                        LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition);
                        if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint))
                        {
                            ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping");
                            return(false);
                        }
                    }
                }
            }

            #endregion

            // test if this turn is part of an intersection
            if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId))
            {
                // intersection
                ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId];

                // check if priority lanes exist for this interconnect
                if (inter.PriorityLanes.ContainsKey(this.turn))
                {
                    // get all the default priority lanes
                    List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn];

                    // get the subpath of the interconnect we care about
                    //LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front);
                    LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position }));                   //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint);

                    // check if path ended
                    if (aiSubpath.Count < 2)
                    {
                        return(true);
                    }

                    // determine all of the new priority lanes
                    List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>();

                    #region Determine new priority areas given position

                    // loop through old priorities
                    foreach (IntersectionInvolved ii in priorities)
                    {
                        // check ii lane
                        if (ii.Area is ArbiterLane)
                        {
                            #region Lane Intersects Turn Path w/ Point of No Return

                            // check if the waypoint is not the last waypoint in the lane
                            if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null)
                            {
                                // check where line intersects path
                                Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric);

                                // check for an intersection
                                if (intersect.HasValue)
                                {
                                    // distance to intersection
                                    double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0;

                                    // determine if we can stop before or after the intersection
                                    double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value);

                                    // check dist to intersection > distance to stoppage
                                    if (distanceToIntersection > distanceToStoppage)
                                    {
                                        // add updated priority
                                        updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)),
                                                                                       ii.Area, ArbiterTurnDirection.Straight));
                                    }
                                    else
                                    {
                                        ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString());
                                    }
                                }
                            }

                            #endregion

                            // we know there is an exit and it is the last waypoint of the segment, fuxk!
                            else
                            {
                                #region Turns Intersect

                                // get point to look at if exists
                                ArbiterInterconnect interconnect;
                                Coordinates?        intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect);

                                // check for the intersect
                                if (intersect.HasValue)
                                {
                                    ArbiterLane al = (ArbiterLane)ii.Area;
                                    LinePath    lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position));
                                    lp.Add(interconnect.InterconnectPath.EndPoint.Location);

                                    // get our time to the intersection point
                                    //double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value));

                                    // get our time to the intersection point
                                    double ourSpeed         = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value);
                                    double stoppedTime      = ourSpeed < 1.0 ? 1.5 : 0.0;
                                    double extraTime        = 1.5;
                                    double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed;
                                    double ourTime          = Math.Min(6.5, stoppedTime + extraTime + interconnectTime);

                                    // get closest vehicle in that lane to the intersection
                                    List <VehicleAgent> toLook = new List <VehicleAgent>();
                                    if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area))
                                    {
                                        foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area])
                                        {
                                            double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position);
                                            if (upstreamDist > 0 && tmpVa.PassedDelayedBirth)
                                            {
                                                toLook.Add(tmpVa);
                                            }
                                        }
                                    }
                                    if (TacticalDirector.VehicleAreas.ContainsKey(interconnect))
                                    {
                                        toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]);
                                    }

                                    // check length of stuff to look at
                                    if (toLook.Count > 0)
                                    {
                                        foreach (VehicleAgent va in toLook)
                                        {
                                            // distance along path to location of intersect
                                            double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location));

                                            double speed  = va.Speed == 0.0 ? 0.01 : va.Speed;
                                            double vaTime = distToIntersect / Math.Abs(speed);
                                            if (vaTime > 0 && vaTime < ourTime)
                                            {
                                                ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO");
                                                return(false);
                                            }
                                        }
                                    }
                                }

                                #endregion
                            }
                        }
                    }

                    #endregion

                    #region Updated Priority Intersection Code

                    // loop through updated priorities
                    bool updatedPrioritiesClear = true;
                    foreach (IntersectionInvolved ii in updatedPriorities)
                    {
                        // lane
                        ArbiterLane al = (ArbiterLane)ii.Area;

                        // get our time to the intersection point
                        double ourSpeed         = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value);
                        double stoppedTime      = ourSpeed < 1.0 ? 1.5 : 0.0;
                        double extraTime        = 1.5;
                        double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed;
                        double ourTime          = Math.Min(6.5, stoppedTime + extraTime + interconnectTime);

                        // double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value));

                        // get closest vehicle in that lane to the intersection
                        if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area))
                        {
                            // get lane vehicles
                            List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area];

                            // determine for all
                            VehicleAgent closestLaneVa     = null;
                            double       closestDistanceVa = Double.MaxValue;
                            double       closestTime       = Double.MaxValue;
                            foreach (VehicleAgent testVa in vas)
                            {
                                // check upstream
                                double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position);

                                // get speed
                                double speed = testVa.Speed;
                                double time  = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed;

                                // check distance > 0
                                if (distance > 0)
                                {
                                    // check if closer or none other exists
                                    if (closestLaneVa == null || time < closestTime)
                                    {
                                        closestLaneVa     = testVa;
                                        closestDistanceVa = distance;
                                        closestTime       = time;
                                    }
                                }
                            }

                            // check if closest exists
                            if (closestLaneVa != null)
                            {
                                // set va
                                VehicleAgent va       = closestLaneVa;
                                double       distance = closestDistanceVa;

                                // check dist and birth time
                                if (distance > 0 && va.PassedDelayedBirth)
                                {
                                    // check time
                                    double speed = va.Speed == 0.0 ? 0.01 : va.Speed;
                                    double time  = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed;

                                    // too close
                                    if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) &&
                                        distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) &&
                                        CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20)
                                    {
                                        ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping");
                                        //return false;
                                        updatedPrioritiesClear = false;
                                        return(false);
                                    }
                                    else if (time > 0 && time < ourTime)
                                    {
                                        ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2"));
                                        //return false;
                                        updatedPrioritiesClear = false;
                                        return(false);
                                    }
                                    else
                                    {
                                        ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2"));
                                        //return true;
                                    }
                                }
                            }
                            else
                            {
                                ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles");
                            }
                        }
                    }
                    return(updatedPrioritiesClear);

                    #endregion
                }
            }

            // fall through fine to go
            ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream");
            return(true);
        }