/// <summary>
        /// Updates the montitor with the closest forward vehicle along the lane
        /// </summary>
        /// <param name="vehicleState"></param>
        /// <param name="final"></param>
        public void Update(VehicleState vehicleState, ArbiterWaypoint final, LinePath fullPath)
        {
            this.VehiclesToIgnore = new List <int>();

            if (TacticalDirector.VehicleAreas.ContainsKey(final.Lane))
            {
                List <VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[final.Lane];

                this.CurrentVehicle = null;
                double currentDistance = Double.MaxValue;

                foreach (VehicleAgent va in laneVehicles)
                {
                    double endDistance = final.Lane.DistanceBetween(final.Position, va.ClosestPosition);
                    if (endDistance > 0)
                    {
                        this.VehiclesToIgnore.Add(va.VehicleId);
                    }

                    double tmpDist = endDistance;

                    if (tmpDist > 0 && (this.CurrentVehicle == null || tmpDist < currentDistance))
                    {
                        this.CurrentVehicle = va;
                        currentDistance     = tmpDist;
                    }
                }
            }
            else
            {
                this.CurrentVehicle = null;
            }
        }
示例#2
0
        public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
        {
            Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);

            vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
            List <Coordinates>   pointsOutside = new List <Coordinates>();
            ArbiterLanePartition alp           = al.GetClosestPartition(va.ClosestPosition);

            foreach (Coordinates c in vehiclePoly)
            {
                if (!p.IsInside(c))
                {
                    pointsOutside.Add(c);
                }
            }

            foreach (Coordinates m in pointsOutside)
            {
                foreach (Coordinates n in pointsOutside)
                {
                    if (!m.Equals(n))
                    {
                        if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
                            GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
                        {
                            return(false);
                        }
                    }
                }
            }

            return(true);
        }
示例#3
0
        /// <summary>
        /// Updates the current vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        public void Update(IFQMPlanable lane, VehicleState state)
        {
            // get the forward path
            LinePath p = lane.LanePath();

            // get our position
            Coordinates f = state.Front;

            // get all vehicles associated with those components
            List <VehicleAgent> vas = new List <VehicleAgent>();

            foreach (IVehicleArea iva in lane.AreaComponents)
            {
                if (TacticalDirector.VehicleAreas.ContainsKey(iva))
                {
                    vas.AddRange(TacticalDirector.VehicleAreas[iva]);
                }
            }

            // get the closest forward of us
            double       minDistance = Double.MaxValue;
            VehicleAgent closest     = null;

            // set the vehicles to ignore
            this.VehiclesToIgnore = new List <int>();

            // get clsoest
            foreach (VehicleAgent va in vas)
            {
                // get position of front
                Coordinates frontPos = va.ClosestPosition;

                // check relatively inside
                if (lane.RelativelyInside(frontPos))
                {
                    // distance to vehicle
                    double frontDist = lane.DistanceBetween(f, frontPos);

                    // check forward of us
                    if (frontDist > 0)
                    {
                        // add to ignorable
                        this.VehiclesToIgnore.Add(va.VehicleId);

                        // check for closest
                        if (frontDist < minDistance)
                        {
                            minDistance = frontDist;
                            closest     = va;
                        }
                    }
                }
            }

            this.CurrentVehicle  = closest;
            this.currentDistance = minDistance;
        }
        /// <summary>
        /// Resets values held over time
        /// </summary>
        public void Reset()
        {
            if (this.CurrentVehicle != null)
            {
                this.CurrentVehicle.Reset();
            }

            this.CurrentVehicle  = null;
            this.currentDistance = Double.MaxValue;
        }
 void Awake()
 {
     if (target == null)
     {
         target = transform.Find("Forklift Target");
     }
     if (agent == null)
     {
         agent = transform.Find("Forklift Agent Inference").GetComponent <VehicleAgent>();
     }
 }
示例#6
0
        public bool VehicleAllInsidePolygon(VehicleAgent va, Polygon p, VehicleState ourState)
        {
            for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
            {
                Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
                if (!p.IsInside(c))
                {
                    return(false);
                }
            }

            return(true);
        }
示例#7
0
    private void Awake()
    {
        if (env == null)
        {
            env = GetComponentInParent <RandomEnvironment>();
        }
        if (agent == null)
        {
            agent = transform.parent.GetComponentInChildren <VehicleAgent>();
        }
        if (background == null)
        {
            background = GetComponentInChildren <Transform>();
        }

        // Position overview at border (in corner?)
    }
示例#8
0
        public void Update(VehicleState ourState)
        {
            if (TacticalDirector.VehicleAreas.ContainsKey(this.finalWaypoint.Perimeter.Zone))
            {
                VehicleAgent closest = null;
                double       tmpDist = Double.MaxValue;

                foreach (VehicleAgent va in TacticalDirector.VehicleAreas[this.finalWaypoint.Perimeter.Zone])
                {
                    if (this.entryPolygon.IsInside(va.ClosestPosition))
                    {
                        double tmp = va.ClosestPosition.DistanceTo(this.finalWaypoint.Position);
                        if (closest == null || tmp < tmpDist)
                        {
                            tmpDist = tmp;
                            closest = va;
                        }
                    }
                }

                if (closest != null && closest.IsStopped)
                {
                    if (this.currentVehicle == null || !this.currentVehicle.Equals(closest))
                    {
                        this.currentVehicle = closest;
                        this.ResetTiming();
                        this.failedTimer.Start();
                    }
                    else
                    {
                        this.currentVehicle = closest;
                    }
                }
                else
                {
                    this.currentVehicle = closest;
                    this.ResetTiming();
                }
            }
            else
            {
                this.currentVehicle = null;
                this.ResetTiming();
            }
        }
        /// <summary>
        /// Updates the current vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        public void Update(ArbiterLane lane, VehicleState state)
        {
            // get the forward path
            LinePath p = lane.LanePath().Clone();

            p.Reverse();

            // get our position
            Coordinates f = state.Front;

            // get all vehicles associated with those components
            List <VehicleAgent> vas = new List <VehicleAgent>();

            foreach (IVehicleArea iva in lane.AreaComponents)
            {
                if (TacticalDirector.VehicleAreas.ContainsKey(iva))
                {
                    vas.AddRange(TacticalDirector.VehicleAreas[iva]);
                }
            }

            // get the closest forward of us
            double       minDistance = Double.MaxValue;
            VehicleAgent closest     = null;

            // get clsoest
            foreach (VehicleAgent va in vas)
            {
                // get position of front
                Coordinates frontPos = va.ClosestPosition;

                // gets distance from other vehicle to us along the lane
                double frontDist = lane.DistanceBetween(frontPos, f);

                if (frontDist >= 0 && frontDist < minDistance)
                {
                    minDistance = frontDist;
                    closest     = va;
                }
            }

            this.CurrentVehicle  = closest;
            this.currentDistance = minDistance;
        }
        /// <summary>
        /// Helps with parameterizations for lateral reasoning
        /// </summary>
        /// <param name="referenceLane"></param>
        /// <param name="fqmLane"></param>
        /// <param name="goal"></param>
        /// <param name="vehicleFront"></param>
        /// <returns></returns>
        public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane,
                                                          Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va)
        {
            // get traveling parameterized list
            List <TravelingParameters> tps = new List <TravelingParameters>();

            // get distance to the goal
            double goalDistance;
            double goalSpeed;

            this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance);
            tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state));

            // get next stop
            ArbiterWaypoint stopPoint;
            double          stopSpeed;
            double          stopDistance;
            StopType        stopType;

            this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List <ArbiterWaypoint>(),
                              out stopPoint, out stopSpeed, out stopDistance, out stopType);
            this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { },
                                out stopSpeed, out stopDistance);
            tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state));

            // get vehicle
            if (va != null)
            {
                VehicleAgent tmp     = this.ForwardVehicle.CurrentVehicle;
                double       tmpDist = this.ForwardVehicle.currentDistance;
                this.ForwardVehicle.CurrentVehicle  = va;
                this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition);
                TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List <ArbiterWaypoint>());
                tps.Add(tp);
                this.ForwardVehicle.CurrentVehicle  = tmp;
                this.ForwardVehicle.currentDistance = tmpDist;
            }

            // parameterize
            tps.Sort();
            return(tps[0]);
        }
        /// <summary>
        /// Update the rear monitor with the closest vehicle in the rear
        /// </summary>
        /// <param name="state"></param>
        public void Update(VehicleState state)
        {
            // get the forward path
            LinePath p = lane.LanePath();

            // get our position
            Coordinates f = state.Front - state.Heading.Normalize(TahoeParams.VL);

            // get all vehicles associated with those components
            List <VehicleAgent> vas = new List <VehicleAgent>();

            foreach (IVehicleArea iva in lane.AreaComponents)
            {
                if (TacticalDirector.VehicleAreas.ContainsKey(iva))
                {
                    vas.AddRange(TacticalDirector.VehicleAreas[iva]);
                }
            }

            // get the closest forward of us
            double       minDistance = Double.MaxValue;
            VehicleAgent closest     = null;

            // get clsoest
            foreach (VehicleAgent va in vas)
            {
                // get position of front
                Coordinates frontPos  = va.ClosestPosition;
                double      frontDist = lane.DistanceBetween(f, frontPos);

                if (frontDist >= 0 && frontDist < minDistance)
                {
                    minDistance = frontDist;
                    closest     = va;
                }
            }

            this.CurrentVehicle  = closest;
            this.currentDistance = minDistance;
        }
        /// <summary>
        /// Update the monitor
        /// </summary>
        public void Update()
        {
            // flag if vehicle exists anymore
            bool contains = false;

            // get updated agent form tactical direction
            foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
            {
                if (this.intersectionVehicle.Equals(va))
                {
                    Polygon vaP = va.GetAbsolutePolygon(CoreCommon.Communications.GetVehicleState());
                    if (this.globalMonitor.Intersection.IntersectionPolygon.IsInside(vaP))
                    {
                        this.intersectionVehicle = va;
                        contains = true;
                    }
                }
            }

            if (contains)
            {
                if (this.intersectionVehicle.StateMonitor.Observed.speedValid && this.intersectionVehicle.IsStopped && !this.stopwatch.IsRunning)
                {
                    this.ResetTimer();
                    this.stopwatch.Start();
                }
                else if (!this.intersectionVehicle.StateMonitor.Observed.speedValid ||
                         !this.intersectionVehicle.StateMonitor.Observed.isStopped)
                {
                    this.ResetTimer();
                }
            }
            else
            {
                this.intersectionVehicle = null;
                this.ResetTimer();
            }
        }
示例#13
0
 // Start is called before the first frame update
 protected override void Start()
 {
     base.Start();
     robotArmAgent = GetComponentInChildren <RobotArmAgent>();
     vehicleAgent  = GetComponentInChildren <VehicleAgent>();
     vehicleAgent.OnTargetEnter = (Transform target) => {
         robotArmAgent.SetTarget(target);
     };
     vehicleAgent.OnTargetExit = (Transform target) => {
         if (!robotArmAgent.robotArm.IsHoldingObject())
         {
             robotArmAgent.SetTarget(null);
         }
     };
     robotArmAgent.OnTargetDroppedSuccessfully = (Transform target) => {
         /* target.GetComponentInParent<Rigidbody>().Sleep();
          * target.GetComponentInParent<Rigidbody>().transform.parent = transform;
          * target.localPosition = new Vector3 (
          *  Random.Range (-4f, 4f),
          *  0f,
          *  Random.Range (4f, 4f)
          * ); */
     };
 }
    // Update is called once per frame
    void Update()
    {
        VehicleAgent vehicle = gameObject.GetComponent <VehicleAgent> ();

        if (Input.GetKey(forwardKey))
        {
            vehicle.Accelerate();
        }

        if (Input.GetKey(backwardKey))
        {
            vehicle.Brake();
        }

        if (Input.GetKey(leftKey))
        {
            vehicle.SteerLeft();
        }

        if (Input.GetKey(rightKey))
        {
            vehicle.SteerRight();
        }
    }
        /// <summary>
        /// Gets closest vehicle on lane before either the exit or where we are clsoest
        /// </summary>
        /// <param name="ourState"></param>
        public void Update(VehicleState ourState)
        {
            #region Exit Polygon

            // check the exit polygon
            if (exit != null)
            {
                // create the polygon
                Polygon p = this.exitPolygon;

                // closest vehicle
                VehicleAgent closest     = null;
                double       closestDist = Double.MinValue;

                // check all inside the polygon
                foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
                {
                    // check if stopped
                    if (va.IsStopped)                    // && va.StateMonitor.Observed.speedValid)
                    {
                        // get distance to exit
                        double      distToExit;
                        Coordinates c = va.ClosestPointTo(this.exit.Position, ourState, out distToExit).Value;

                        // check inside polygon
                        if (p.IsInside(c))
                        {
                            // check dist
                            if (closest == null || distToExit < closestDist)
                            {
                                closest     = va;
                                closestDist = distToExit;
                            }
                        }
                    }
                }

                // check closest not null
                if (closest != null)
                {
                    // check vehicle switch
                    if (currentVehicle != closest)
                    {
                        this.ResetTiming();
                    }

                    // check if waiting timer not running
                    if (!this.waitingTimer.IsRunning)
                    {
                        this.ResetTiming();
                        this.waitingTimer.Start();
                    }

                    // set the new vehicle
                    this.currentVehicle = closest;

                    // return to stop update
                    return;
                }
                else
                {
                    // check if we were waiting for something in this area
                    if (this.globalMonitor.PreviouslyWaiting.Contains(this))
                    {
                        // remove the previous waiting
                        this.globalMonitor.PreviouslyWaiting.Remove(this);
                    }
                }
            }

            #endregion

            #region Lane Vehicles

            // get the possible vehicles that could be in this stop
            if (TacticalDirector.VehicleAreas.ContainsKey(this.lane))
            {
                // get vehicles in the the lane of the exit
                List <VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[this.lane];

                // get the point we are looking from
                LinePath.PointOnPath referencePoint;

                // reference from exit if exists
                if (exit != null)
                {
                    LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(exit.Position);
                    referencePoint = lane.LanePath().AdvancePoint(pop, 3.0);
                }
                // otherwise look from where we are closest
                else
                {
                    referencePoint = lane.LanePath().GetClosestPoint(ourState.Front);
                }

                // tmp
                VehicleAgent tmp     = null;
                double       tmpDist = Double.MaxValue;

                // check over all possible vehicles in the lane
                foreach (VehicleAgent possible in possibleVehicles)
                {
                    // get vehicle point on lane
                    LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);

                    // check if the vehicle is behind the reference point
                    double vehicleDist = lane.LanePath().DistanceBetween(vehiclePoint, referencePoint);

                    // check for closest
                    if (vehicleDist > 0 && (tmp == null || vehicleDist < tmpDist))
                    {
                        tmp     = possible;
                        tmpDist = vehicleDist;
                    }
                }

                // set the closest
                this.currentVehicle = tmp;
            }
            else
            {
                // set the closest
                this.currentVehicle = null;
            }

            #endregion

            // reset the timing if got this far
            this.ResetTiming();
        }
        /// <summary>
        /// Plan a lane change
        /// </summary>
        /// <param name="cls"></param>
        /// <param name="initialManeuver"></param>
        /// <param name="targetManeuver"></param>
        /// <returns></returns>
        public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan,
                                       List <ITacticalBlockage> blockages, List <ArbiterWaypoint> ignorable)
        {
            // check blockages
            if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
            {
                // create the blockage state
                EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                // go to a blockage handling tactical
                return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp));
            }

            // lanes of the lane change
            ArbiterLane initial = cls.Parameters.Initial;
            ArbiterLane target  = cls.Parameters.Target;

            #region Initial Forwards

            if (!cls.Parameters.InitialOncoming)
            {
                ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial);

                #region Target Forwards

                if (!cls.Parameters.TargetOncoming)
                {
                    // target reasoning
                    ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);

                    #region Navigation

                    if (cls.Parameters.Reason == LaneChangeReason.Navigation)
                    {
                        // parameters to follow
                        List <TravelingParameters> tps = new List <TravelingParameters>();

                        // vehicles to ignore
                        List <int> ignorableVehicles = new List <int>();

                        // params for forward lane
                        initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
                                                                                                                   CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                                                                                                                   initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                                                                                                   vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);

                        ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3"));

                        if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                        {
                            tps.Add(initialParams);
                        }
                        else
                        {
                            tps.Add(initialReasoning.ForwardMonitor.NavigationParameters);
                        }

                        ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);

                        // get params for the final lane
                        targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List <ArbiterWaypoint>());
                        TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
                        tps.Add(targetParams);
                        ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);

                        try
                        {
                            if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 &&
                                targetParams.Type == TravellingType.Vehicle &&
                                targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null &&
                                targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
                            {
                                return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
                            }
                        }
                        catch (Exception) { }

                        ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3"));

                        // decorators
                        List <BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;

                        // distance
                        double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // check if need to modify distance to go
                        if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                        {
                            double curDistToUpper    = cls.Parameters.DistanceToDepartUpperBound;
                            double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0;

                            if (curDistToUpper > newVhcDistToUpper)
                            {
                                distanceToGo = newVhcDistToUpper;
                            }
                        }

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
                        if (sc.Speed < 8.84)
                        {
                            sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84));
                        }

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
                                                                        sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
                                                                        initial.NumberOfLanesRight(vehicleState.Front, true));

                        // standard maneuver
                        return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
                    }

                    #endregion

                    #region Failed Forwards

                    else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
                    {
                        // parameters to follow
                        List <TravelingParameters> tps = new List <TravelingParameters>();

                        // vehicles to ignore
                        List <int> ignorableVehicles = new List <int>();

                        // params for forward lane
                        initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
                                                                                                                   CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                                                                                                                   initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                                                                                                   vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
                        tps.Add(initialParams);
                        ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);

                        // get params for the final lane
                        targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List <ArbiterWaypoint>());
                        TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
                        tps.Add(targetParams);
                        ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);

                        // decorators
                        List <BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;

                        // distance
                        double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
                                                                        sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
                                                                        initial.NumberOfLanesRight(vehicleState.Front, true));

                        // standard maneuver
                        return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
                    }

                    #endregion

                    #region Slow

                    else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }

                    #endregion

                    else
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                }

                #endregion

                #region Target Oncoming

                else
                {
                    OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target);

                    #region Failed Forward

                    if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
                    {
                        // parameters to follow
                        List <TravelingParameters> tps = new List <TravelingParameters>();

                        // ignore the forward vehicle but keep params for forward lane
                        initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
                                                                                                                   CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                                                                                                                   initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
                        tps.Add(initialParams);

                        // get params for the final lane
                        targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages);
                        TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value;
                        tps.Add(targetParams);

                        // decorators
                        List <BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;

                        // distance
                        double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24));

                        // check final for stopped failed opposing
                        VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle;
                        if (forwardVa != null)
                        {
                            // dist between
                            double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);

                            // check stopped
                            bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;

                            // check distance
                            bool distOk = distToFV < 2.5 * TahoeParams.VL;

                            // check failed
                            bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;

                            // notify
                            ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());

                            // check all for failed
                            if (stopped && distOk && failed)
                            {
                                // check inside target
                                if (target.LanePolygon.IsInside(vehicleState.Front))
                                {
                                    // blockage recovery
                                    StayInLaneState       sils = new StayInLaneState(initial, CoreCommon.CorePlanningState);
                                    StayInLaneBehavior    silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List <int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false));
                                    BlockageRecoveryState brs  = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE,
                                                                                           new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None),
                                                                                           BlockageRecoverySTATUS.EXECUTING);
                                    return(new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp));
                                }
                                // check which lane we are in
                                else
                                {
                                    // return to forward lane
                                    return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
                                }
                            }
                        }

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
                                                                        sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
                                                                        initial.NumberOfLanesRight(vehicleState.Front, true));

                        // standard maneuver
                        return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
                    }

                    #endregion

                    #region Other

                    else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                    else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                    else
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }

                    #endregion
                }

                #endregion
            }

            #endregion

            #region Initial Oncoming

            else
            {
                OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial);

                #region Target Forwards

                if (!cls.Parameters.TargetOncoming)
                {
                    ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);

                    if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }

                    #region Navigation

                    else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
                    {
                        // parameters to follow
                        List <TravelingParameters> tps = new List <TravelingParameters>();

                        // distance to the upper bound of the change
                        double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // get params for the initial lane
                        initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages);

                        // current params of the fqm
                        TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value;

                        if (initialParams.Type == TravellingType.Vehicle)
                        {
                            if (!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                            {
                                tps.Add(initialParams);
                            }
                            else
                            {
                                tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
                                distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL;
                            }
                        }
                        else
                        {
                            tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
                        }

                        // get params for forward lane
                        targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target,
                                                                                                                 CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                                                                                                                 target.WaypointList[target.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                                                                                                 vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
                        tps.Add(targetParams);

                        // ignoring vehicles add
                        List <int> ignoreVehicles = initialParams.VehiclesToIgnore;
                        ignoreVehicles.AddRange(targetParams.VehiclesToIgnore);

                        // decorators
                        List <BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator;

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        SpeedCommand sc = tps[0].SpeedCommand;

                        if (sc is StopAtDistSpeedCommand)
                        {
                            sc = new ScalarSpeedCommand(0.0);
                        }

                        // check final for stopped failed opposing
                        VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle;
                        if (forwardVa != null)
                        {
                            // dist between
                            double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);

                            // check stopped
                            bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;

                            // check distance
                            bool distOk = distToFV < 2.5 * TahoeParams.VL;

                            // check failed
                            bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;

                            // notify
                            ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());

                            // check all for failed
                            if (stopped && distOk && failed)
                            {
                                // check which lane we are in
                                if (initial.LanePolygon.IsInside(vehicleState.Front))
                                {
                                    // return to opposing lane
                                    return(new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
                                }
                                else
                                {
                                    // lane state
                                    return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
                                }
                            }
                        }

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
                                                                        sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false),
                                                                        initial.NumberOfLanesRight(vehicleState.Front, false));

                        // standard maneuver
                        return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
                    }

                    #endregion

                    else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                    else
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                }

                #endregion

                else
                {
                    // fallout exception
                    throw new Exception("currently unsupported lane change type");
                }
            }

            #endregion
        }
示例#17
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);
        }
示例#18
0
        /// <summary>
        /// Populates mapping of vehicles we want to track given state
        /// </summary>
        /// <param name="vehicles"></param>
        public void PopulateValidVehicles(SceneEstimatorTrackedClusterCollection vehicles)
        {
            TacticalDirector.NewVehicles      = new List <VehicleAgent>();
            TacticalDirector.OccludedVehicles = new Dictionary <int, VehicleAgent>();

            if (TacticalDirector.ValidVehicles == null)
            {
                TacticalDirector.ValidVehicles = new Dictionary <int, VehicleAgent>();
            }

            List <int> toRemove = new List <int>();

            foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
            {
                bool found = false;
                for (int i = 0; i < vehicles.clusters.Length; i++)
                {
                    if (vehicles.clusters[i].id.Equals(va.VehicleId))
                    {
                        found = true;
                    }
                }

                if (!found)
                {
                    toRemove.Add(va.VehicleId);
                }
            }
            foreach (int r in toRemove)
            {
                TacticalDirector.ValidVehicles.Remove(r);
            }

            for (int i = 0; i < vehicles.clusters.Length; i++)
            {
                SceneEstimatorTrackedCluster ov = vehicles.clusters[i];
                bool clusterStopped             = ov.isStopped;
                if (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE ||
                    (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && !clusterStopped) ||
                    (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && !clusterStopped))
                {
                    if (ov.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE)
                    {
                        if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
                        {
                            TacticalDirector.ValidVehicles[ov.id].StateMonitor.Observed = ov;
                        }
                        else
                        {
                            VehicleAgent ovNew = new VehicleAgent(ov);
                            TacticalDirector.ValidVehicles.Add(ovNew.VehicleId, ovNew);
                            TacticalDirector.NewVehicles.Add(ovNew);
                        }
                    }
                    else
                    {
                        if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
                        {
                            TacticalDirector.ValidVehicles.Remove(ov.id);
                        }
                    }
                }
                else
                {
                    if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
                    {
                        TacticalDirector.ValidVehicles.Remove(ov.id);
                    }
                }

                if ((ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && clusterStopped) ||
                    (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && clusterStopped))
                {
                    TacticalDirector.OccludedVehicles.Add(ov.id, new VehicleAgent(ov));
                }
            }
        }
示例#19
0
 void Start()
 {
     vehicle = gameObject.GetComponent <VehicleAgent>();
 }
示例#20
0
 // Use this for initialization
 void Awake()
 {
     vehicle   = gameObject.GetComponent <VehicleAgent>();
     navigator = gameObject.GetComponent <NavigationAgent>();
 }
示例#21
0
        /// <summary>
        /// Updates this monitor
        /// </summary>
        public void Update(VehicleState ourState)
        {
            // make sure not our vehicle
            if (!isOurs)
            {
                // get a list of vehicles possibly in the exit
                List <VehicleAgent> stopVehicles = new List <VehicleAgent>();

                // check all valid vehicles
                foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
                {
                    // check if any of the point inside the polygon
                    for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
                    {
                        // get abs coord
                        Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);

                        // check if inside poly
                        if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
                        {
                            // add vehicle
                            stopVehicles.Add(va);
                        }
                    }
                }

                // check occluded
                foreach (VehicleAgent va in TacticalDirector.OccludedVehicles.Values)
                {
                    // check if any of the point inside the polygon
                    for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
                    {
                        // get abs coord
                        Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);

                        // check if inside poly
                        if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
                        {
                            // add vehicle
                            ArbiterOutput.Output("Added occluded vehicle: " + va.ToString() + " to SEM: " + this.stoppedExit.ToString());
                            stopVehicles.Add(va);
                        }
                    }
                }

                // check if we already have a vehicle we are referencing
                if (this.currentVehicle != null && !stopVehicles.Contains(this.currentVehicle))
                {
                    // get the polygon of the current vehicle	and inflate a tiny bit?
                    Polygon p = this.currentVehicle.GetAbsolutePolygon(ourState);
                    p = p.Inflate(1.0);

                    // Vehicle agent to switch to if exists
                    VehicleAgent newer    = null;
                    double       distance = Double.MaxValue;

                    // check for closest vehicle to exist in that polygon
                    foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
                    {
                        // check if vehicle is stopped
                        if (stopVehicles.Contains(va))
                        {
                            // get distance to stop
                            double stopLineDist;
                            va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out stopLineDist);

                            // check all vehicle points
                            foreach (Coordinates rel in va.StateMonitor.Observed.relativePoints)
                            {
                                Coordinates abs     = va.TransformCoordAbs(rel, ourState);
                                double      tmpDist = stopLineDist;

                                // check for the clsoest with points inside vehicle
                                if (p.IsInside(abs) && (newer == null || tmpDist < distance))
                                {
                                    newer    = va;
                                    distance = tmpDist;
                                }
                            }
                        }
                    }

                    // check if we can switch vehicles
                    if (newer != null)
                    {
                        ArbiterOutput.Output("For StopLine: " + this.stoppedExit.ToString() + " switched vehicleId: " + this.currentVehicle.VehicleId.ToString() + " for vehicleId: " + newer.VehicleId.ToString());
                        this.currentVehicle   = newer;
                        this.NotFoundPrevious = notFoundDefault;
                    }
                    else if (NotFoundPrevious == 0)
                    {
                        // set this as not having a vehicle
                        this.currentVehicle = null;

                        // set not found before
                        this.NotFoundPrevious = this.notFoundDefault;

                        // stop timers
                        this.timer.Stop();
                        this.timer.Reset();
                    }
                    else
                    {
                        // set not found before
                        this.NotFoundPrevious--;
                        ArbiterOutput.Output("current not found, not found previous: " + this.NotFoundPrevious.ToString() + ", at stopped exit: " + this.stoppedExit.Waypoint.ToString());
                    }
                }
                // update the current vehicle
                else if (this.currentVehicle != null && stopVehicles.Contains(this.currentVehicle))
                {
                    // udpate current
                    this.currentVehicle = stopVehicles[stopVehicles.IndexOf(this.currentVehicle)];

                    // set as found previous
                    this.NotFoundPrevious = this.notFoundDefault;

                    // check if need to update timer
                    if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                        this.timer.Start();
                    }
                    // otherwise check if moving
                    else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                    }
                }
                // otherwise if we have no vehicle and exist vehicles in stop area
                else if (this.currentVehicle == null && stopVehicles.Count > 0)
                {
                    // get closest vehicle to the stop
                    VehicleAgent toTrack  = null;
                    double       distance = Double.MaxValue;

                    // set as found previous
                    this.NotFoundPrevious = this.notFoundDefault;

                    // loop through
                    foreach (VehicleAgent va in stopVehicles)
                    {
                        // check if vehicle is stopped
                        if (va.IsStopped)
                        {
                            // distance of vehicle to stop line
                            double      distanceToStop;
                            Coordinates closestToStop = va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out distanceToStop).Value;

                            // check if we don't have one or tmp closer than tracked
                            if (toTrack == null || distanceToStop < distance)
                            {
                                toTrack  = va;
                                distance = distanceToStop;
                            }
                        }
                    }

                    // check if we have one to track
                    if (toTrack != null)
                    {
                        this.currentVehicle = toTrack;
                    }
                }
            }
        }
示例#22
0
    private void Awake()
    {
        _timer = new Timer(() => Time.time);

        _vehicleAgent = GetComponent <VehicleAgent>();
    }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="reason"></param>
 /// <param name="forwardVehicle"></param>
 public LaneChangeInformation(LaneChangeReason reason, VehicleAgent forwardVehicle)
 {
     this.Reason         = reason;
     this.ForwardVehicle = forwardVehicle;
 }
示例#24
0
        /// <summary>
        /// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool Occupied(ArbiterLane lane, VehicleState state)
        {
            // check stopwatch time for proper elapsed
            if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10)
            {
                this.Reset();
            }

            if (TacticalDirector.VehicleAreas.ContainsKey(lane))
            {
                // vehicles in the lane
                List <VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane];

                // position of the center of our vehicle
                Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);

                // cutoff for allowing vehicles outside this range
                double dCutOff = TahoeParams.VL * 1.5;

                // loop through vehicles
                foreach (VehicleAgent va in laneVehicles)
                {
                    // vehicle high level distance
                    double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition));

                    // check less than distance cutoff
                    if (d < dCutOff)
                    {
                        this.Reset();
                        this.CurrentVehicle = va;
                        ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
                        return(true);
                    }
                }
            }

            // now check the lateral sensor for being occupied
            if (this.SideSickObstacleDetected(lane, state))
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                this.Reset();
                ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
                return(true);
            }

            // none found
            this.CurrentVehicle = null;

            // if none found, tiemr not running start timer
            if (!this.LateralClearStopwatch.IsRunning)
            {
                this.Reset();
                this.LateralClearStopwatch.Start();
                ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
                return(true);
            }
            // enough time
            else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 1.0)
            {
                ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
                return(false);
            }
            // not enough time
            else
            {
                double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
                ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
                return(true);
            }
        }
 protected override void Start()
 {
     base.Start();
     vehicleAgent = GetComponentInChildren <VehicleAgent>();
 }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="vehicle"></param>
 public IntersectionVehicleMonitor(IntersectionMonitor global, VehicleAgent vehicle)
 {
     this.stopwatch           = new Stopwatch();
     this.intersectionVehicle = vehicle;
     this.globalMonitor       = global;
 }
示例#27
0
        public void Update(UrbanChallenge.Common.Vehicle.VehicleState ourState)
        {
            // get lane
            ArbiterLane lane = this.turnFinalWaypoint.Lane;

            // get the possible vehicles that could be in this stop
            if (TacticalDirector.VehicleAreas.ContainsKey(lane))
            {
                // get vehicles in the the lane of the exit
                List <VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[lane];

                // get the point we are looking from
                LinePath.PointOnPath referencePoint = lane.LanePath().GetClosestPoint(this.turnFinalWaypoint.Position);

                // tmp
                VehicleAgent tmp     = null;
                double       tmpDist = Double.MaxValue;

                // check over all possible vehicles
                foreach (VehicleAgent possible in possibleVehicles)
                {
                    // get vehicle point on lane
                    LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);

                    // check if the vehicle is in front of the reference point
                    double vehicleDist = lane.LanePath().DistanceBetween(referencePoint, vehiclePoint);
                    if (vehicleDist >= 0 && vehicleDist < TahoeParams.VL * 2.0)
                    {
                        tmp     = possible;
                        tmpDist = vehicleDist;
                    }
                }

                if (tmp != null)
                {
                    if (this.currentVehicle == null || !this.currentVehicle.Equals(tmp))
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                        this.currentVehicle = tmp;
                    }
                    else
                    {
                        this.currentVehicle = tmp;
                    }

                    if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                        this.timer.Start();
                    }
                    else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                    }
                }
                else
                {
                    this.currentVehicle = null;
                    this.timer.Stop();
                    this.timer.Reset();
                }
            }
            else
            {
                this.timer.Stop();
                this.timer.Reset();
                this.currentVehicle = null;
            }
        }
 /// <summary>
 /// Resets values held over time
 /// </summary>
 public void Reset()
 {
     this.CurrentVehicle  = null;
     this.currentDistance = 0.0;
     this.ResetTiming();
 }