Esempio n. 1
0
        public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight,
                                        LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp,
                                        out ILaneModel startingLaneModel, out ILaneModel endingLaneModel)
        {
            LocalRoadModel localRoadModel = this.localRoadModel;

            // get the selected lane for the starting path
            int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight);

            // calculate the number of lanes left and right for the ending lane
            int endingNumLanesLeft  = startingNumLanesLeft;
            int endingNumLanesRight = startingNumLanesRight;

            if (changeLeft)
            {
                endingNumLanesLeft--;
                endingNumLanesRight++;
            }
            else
            {
                endingNumLanesLeft++;
                endingNumLanesRight--;
            }

            // get the selected lane for the ending path
            int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight);

            // check if either is invalid or the difference does not line up with the change direction
            int deltaExpected = changeLeft ? 1 : -1;             // starting - ending

            if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected)
            {
                // this is some invalid stuff
                // we won't use either of them since we're not sure which one is truly valid
                startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
                endingLaneModel   = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);

                // mark that we did reject
                didRejectLast = true;
            }
            else
            {
                // looks like the lane model selection was valid
                startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
                endingLaneModel   = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);

                // mark that we did not reject
                didRejectLast = false;
            }

            // figure out what we want to send to the ui
            // for now, just send starting lane model
            SendLaneModelToUI(startingLaneModel, rndfPathTimestamp);
        }
Esempio n. 2
0
        private void SendLaneModelToUI(ILaneModel laneModel, CarTimestamp ts)
        {
            // get the center, left and right bounds
            //LinearizationOptions opts = new LinearizationOptions(0, 50, ts);
            //LinePath finalCenter = laneModel.LinearizeCenterLine(opts);
            //LinePath finalLeft = laneModel.LinearizeLeftBound(opts);
            //LinePath finalRight = laneModel.LinearizeRightBound(opts);

            //Services.UIService.PushLineList(finalCenter, ts, "lane center line", true);
            //Services.UIService.PushLineList(finalLeft, ts, "lane left bound", true);
            //Services.UIService.PushLineList(finalRight, ts, "lane right bound", true);
        }
        public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight,
            LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp,
            out ILaneModel startingLaneModel, out ILaneModel endingLaneModel)
        {
            LocalRoadModel localRoadModel = this.localRoadModel;

            // get the selected lane for the starting path
            int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight);

            // calculate the number of lanes left and right for the ending lane
            int endingNumLanesLeft = startingNumLanesLeft;
            int endingNumLanesRight = startingNumLanesRight;

            if (changeLeft) {
                endingNumLanesLeft--;
                endingNumLanesRight++;
            }
            else {
                endingNumLanesLeft++;
                endingNumLanesRight--;
            }

            // get the selected lane for the ending path
            int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight);

            // check if either is invalid or the difference does not line up with the change direction
            int deltaExpected = changeLeft ? 1 : -1; // starting - ending
            if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected) {
                // this is some invalid stuff
                // we won't use either of them since we're not sure which one is truly valid
                startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
                endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);

                // mark that we did reject
                didRejectLast = true;
            }
            else {
                // looks like the lane model selection was valid
                startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp);
                endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp);

                // mark that we did not reject
                didRejectLast = false;
            }

            // figure out what we want to send to the ui
            // for now, just send starting lane model
            SendLaneModelToUI(startingLaneModel, rndfPathTimestamp);
        }
Esempio n. 4
0
        public ILaneModel GetLaneModel(LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp, int numLanesLeft, int numLanesRight)
        {
            LocalRoadModel localRoadModel = this.localRoadModel;

            int selectedLane = SelectLane(localRoadModel, rndfPath, rndfPathWidth, rndfPathTimestamp, numLanesLeft, numLanesRight);

            // send the selected lane index (or rejection code) out
            Services.Dataset.ItemAs <double>("selected lane").Add(selectedLane, rndfPathTimestamp);

            // build the road model from the selected lane
            ILaneModel finalLaneModel = GetLaneModel(localRoadModel, selectedLane, rndfPath, rndfPathWidth, rndfPathTimestamp);

            // send to UI for display
            SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);

            // return back to center
            return(finalLaneModel);
        }
        private void SendLaneModelToUI(ILaneModel laneModel, CarTimestamp ts)
        {
            // get the center, left and right bounds
            //LinearizationOptions opts = new LinearizationOptions(0, 50, ts);
            //LinePath finalCenter = laneModel.LinearizeCenterLine(opts);
            //LinePath finalLeft = laneModel.LinearizeLeftBound(opts);
            //LinePath finalRight = laneModel.LinearizeRightBound(opts);

            //Services.UIService.PushLineList(finalCenter, ts, "lane center line", true);
            //Services.UIService.PushLineList(finalLeft, ts, "lane left bound", true);
            //Services.UIService.PushLineList(finalRight, ts, "lane right bound", true);
        }
Esempio n. 6
0
        public override void Process(object param)
        {
            // inform the base that we're beginning processing
            if (!BeginProcess())
            {
                return;
            }

            // check if we were given a parameter
            if (param != null && param is StayInLaneBehavior)
            {
                HandleBehavior((StayInLaneBehavior)param);
            }

            if (reverseGear)
            {
                ProcessReverse();
                return;
            }

            // figure out the planning distance
            double planningDistance = GetPlanningDistance();

            if (sparse && planningDistance > 10)
            {
                planningDistance = 10;
            }

            double?boundStartDistMin = null;
            double?boundEndDistMax   = null;

            if (laneID != null && Services.RoadNetwork != null)
            {
                ArbiterLane          lane          = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
                AbsolutePose         pose          = Services.StateProvider.GetAbsolutePose();
                ArbiterLanePartition partition     = lane.GetClosestPartition(pose.xy);
                LinePath             partitionPath = partition.UserPartitionPath;
                LinePath.PointOnPath closestPoint  = partitionPath.GetClosestPoint(pose.xy);
                double remainingDist = planningDistance;
                double totalDist     = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint);
                remainingDist -= totalDist;

                if (sparse)
                {
                    // walk ahead and determine where sparsity ends
                    bool nonSparseFound = false;

                    while (remainingDist > 0)
                    {
                        // get the next partition
                        partition = partition.Final.NextPartition;
                        if (partition == null)
                        {
                            break;
                        }

                        if (partition.Type != PartitionType.Sparse)
                        {
                            nonSparseFound = true;
                            break;
                        }
                        else
                        {
                            double dist = partition.UserPartitionPath.PathLength;
                            totalDist     += dist;
                            remainingDist -= dist;
                        }
                    }

                    if (nonSparseFound)
                    {
                        boundStartDistMin = totalDist;
                    }
                }
                else
                {
                    // determine if there is a sparse segment upcoming
                    bool sparseFound = false;
                    while (remainingDist > 0)
                    {
                        partition = partition.Final.NextPartition;

                        if (partition == null)
                        {
                            break;
                        }

                        if (partition.Type == PartitionType.Sparse)
                        {
                            sparseFound = true;
                            break;
                        }
                        else
                        {
                            double dist = partition.Length;
                            totalDist     += dist;
                            remainingDist -= dist;
                        }
                    }

                    if (sparseFound)
                    {
                        boundEndDistMax = totalDist;
                        sparse          = true;
                    }
                }
            }

            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance);

            // update the rndf path
            RelativeTransform relTransfrom    = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
            LinePath          curRndfPath     = rndfPath.Transform(relTransfrom);
            ILaneModel        centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight);

            double avoidanceExtra = sparse ? 5 : 7.5;

            LinePath centerLine, leftBound, rightBound;

            if (boundEndDistMax != null || boundStartDistMin != null)
            {
                LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
            }
            else
            {
                LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound);
            }
            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);

            // calculate time to collision of opposing obstacle if it exists
            LinePath targetLine   = centerLine;
            double   targetWeight = default_lane_alpha_w;

            if (sparse)
            {
                targetWeight = 0.00000025;
            }
            else if (oncomingVehicleExists)
            {
                double shiftDist = -(TahoeParams.T + 1);
                targetLine   = centerLine.ShiftLateral(shiftDist);
                targetWeight = 0.01;
            }
            else if (opposingLaneVehicleExists)
            {
                double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed));
                Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp);
                if (timeToCollision < 5)
                {
                    // shift to the right
                    double shiftDist = -TahoeParams.T / 2.0;
                    targetLine = centerLine.ShiftLateral(shiftDist);
                }
            }

            // set up the planning
            AddTargetPath(targetLine, targetWeight);
            if (!sparse || boundStartDistMin != null || boundEndDistMax != null)
            {
                AddLeftBound(leftBound, true);
                if (!oncomingVehicleExists)
                {
                    AddRightBound(rightBound, false);
                }
            }


            double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance);

            smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
            maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint;

            double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength;

            extraDist = Math.Min(extraDist, 5);

            if (extraDist > 0)
            {
                centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist));
            }
            avoidanceBasePath = centerLine;

            Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
            Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
            Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);

            // get the overall max speed looking forward from our current point
            settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay));
            // get the max speed at the end point
            settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance));
            useAvoidancePath        = false;
            if (sparse)
            {
                // limit to 5 mph
                laneWidthAtPathEnd = 20;
                pathAngleCheckMax  = 50;
                pathAngleMax       = 5 * Math.PI / 180.0;
                settings.maxSpeed  = Math.Min(settings.maxSpeed, 2.2352);
                maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
                //maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);

                LinePath leftEdge  = RoadEdge.GetLeftEdgeLine();
                LinePath rightEdge = RoadEdge.GetRightEdgeLine();
                if (leftEdge != null)
                {
                    leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
                }
                if (rightEdge != null)
                {
                    rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
                }

                PlanningResult            result           = new PlanningResult();
                ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
                if (commandGenerator == null)
                {
                    // we have a block, report dynamically infeasible
                    result = OnDynamicallyInfeasible(null, null);
                }
                else
                {
                    result.steeringCommandGenerator = commandGenerator;
                    result.commandLabel             = commandLabel;
                }
                Track(result, commandLabel);
                return;
            }
            else if (oncomingVehicleExists)
            {
                laneWidthAtPathEnd = 10;
            }

            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);

            disablePathAngleCheck = false;

            SmoothAndTrack(commandLabel, true);
        }
        protected void LinearizeStayInLane(ILaneModel laneModel, double laneDist,
            double? laneStartDistMax, double? boundDistMax, double? boundStartDistMin, double? boundStartDistMax, CarTimestamp curTimestamp,
            out LinePath centerLine, out LinePath leftBound, out LinePath rightBound)
        {
            // get the center line, left bound and right bound
            LinearizationOptions laneOpts = new LinearizationOptions(0, laneDist, curTimestamp);
            centerLine = laneModel.LinearizeCenterLine(laneOpts);
            if (centerLine == null || centerLine.Count < 2) {
                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Error, 0, "center line linearization SUCKS");
            }

            double leftStartDist = vs.speed*TahoeParams.actuation_delay + 1;
            double rightStartDist = vs.speed*TahoeParams.actuation_delay + 1;

            double offtrackDistanceBack = centerLine.ZeroPoint.OfftrackDistance(Coordinates.Zero);
            bool offtrackLeftBack = offtrackDistanceBack > 0;
            double additionalDistBack = 5*Math.Abs(offtrackDistanceBack)/laneModel.Width;
            if (offtrackLeftBack) {
                leftStartDist += additionalDistBack;
            }
            else {
                rightStartDist += additionalDistBack;
            }

            // now determine how to generate the left/right bounds
            // figure out the offtrack distance from the vehicle's front bumper
            double offtrackDistanceFront = centerLine.ZeroPoint.OfftrackDistance(new Coordinates(TahoeParams.FL, 0));

            // offtrackDistance > 0 => we're to left of path
            // offtrackDistance < 0 => we're to right of path
            bool offsetLeftFront = offtrackDistanceFront > 0;

            double additionalDistFront = 10*Math.Abs(offtrackDistanceFront)/laneModel.Width;
            if (offsetLeftFront) {
                leftStartDist += additionalDistFront;
            }
            else {
                rightStartDist += additionalDistFront;
            }

            if (boundStartDistMax.HasValue) {
                if (leftStartDist > boundStartDistMax.Value) {
                    leftStartDist = boundStartDistMax.Value;
                }

                if (rightStartDist > boundStartDistMax.Value) {
                    rightStartDist = boundStartDistMax.Value;
                }
            }

            if (boundStartDistMin.HasValue) {
                if (leftStartDist < boundStartDistMin.Value) {
                    leftStartDist = boundStartDistMin.Value;
                }

                if (rightStartDist < boundStartDistMin.Value) {
                    rightStartDist = boundStartDistMin.Value;
                }
            }

            double boundEndDist = laneDist + 5;
            if (boundDistMax.HasValue && boundDistMax.Value < boundEndDist) {
                boundEndDist = boundDistMax.Value;
            }

            laneOpts = new LinearizationOptions(leftStartDist, boundEndDist, curTimestamp);
            leftBound = laneModel.LinearizeLeftBound(laneOpts);
            laneOpts = new LinearizationOptions(rightStartDist, boundEndDist, curTimestamp);
            rightBound = laneModel.LinearizeRightBound(laneOpts);

            double laneStartDist = TahoeParams.FL;
            if (laneStartDistMax.HasValue && laneStartDistMax.Value < laneStartDist) {
                laneStartDist = laneStartDistMax.Value;
            }

            // remove the first FL distance of the center line
            if (laneStartDist > 0) {
                centerLine = centerLine.RemoveBefore(centerLine.AdvancePoint(centerLine.ZeroPoint, laneStartDist));
                centerLine.Insert(0, Coordinates.Zero);
            }
        }
 protected void LinearizeStayInLane(ILaneModel laneModel, double laneDist, 
     double? laneStartDistMax, double? boundDistMax, double? boundStartDistMax, CarTimestamp curTimestamp,
     out LinePath centerLine, out LinePath leftBound, out LinePath rightBound)
 {
     LinearizeStayInLane(laneModel, laneDist, laneStartDistMax, boundDistMax, null, boundStartDistMax, curTimestamp, out centerLine, out leftBound, out rightBound);
 }