コード例 #1
0
        public LinePath LinearizeCenterLine(LinearizationOptions options)
        {
            LinePath transformedPath = centerlinePath;

            if (options.Timestamp.IsValid)
            {
                RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp);
                OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp);
                transformedPath = centerlinePath.Transform(relTransform);
            }

            LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance);

            LinePath subPath = new LinePath();;

            if (options.EndDistance > options.StartDistance)
            {
                subPath = transformedPath.SubPath(startPoint, options.EndDistance - options.StartDistance);
            }

            if (subPath.Count < 2)
            {
                subPath.Clear();
                Coordinates startPt = startPoint.Location;

                subPath.Add(startPt);
                subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector *Math.Max(options.EndDistance - options.StartDistance, 0.1) + startPt);
            }

            return(subPath);
        }
コード例 #2
0
        protected override bool IsOffRoad()
        {
            // update the rndf path
            RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
            LinePath          curRndfPath  = rndfPath.Transform(relTransfrom);

            LinePath.PointOnPath closestPoint = curRndfPath.ZeroPoint;

            // determine if our heading is off-set from the lane greatly
            LineSegment curSegment    = curRndfPath.GetSegment(closestPoint.Index);
            double      relativeAngle = Math.Abs(curSegment.UnitVector.ArcTan);

            if (relativeAngle > 30 * Math.PI / 180.0)
            {
                return(true);
            }

            // check the offtrack distance
            if (Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) / (rndfPathWidth / 2.0) > 1)
            {
                return(true);
            }

            return(false);
        }
コード例 #3
0
        public DirectionAlong DirectionAlongSegment(LinePath lp)
        {
            // get heading of the lane path there
            Coordinates pathVector = lp.GetSegment(0).UnitVector;

            // get vehicle heading
            Coordinates unit          = new Coordinates(1, 0);
            Coordinates headingVector = unit.Rotate(this.Heading.ArcTan);

            // rotate vehicle heading
            Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan);

            // get path heading
            double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees());

            if (relativeVehicleDegrees < 70)
            {
                return(DirectionAlong.Forwards);
            }
            else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
            {
                return(DirectionAlong.Perpendicular);
            }
            else
            {
                return(DirectionAlong.Reverse);
            }
        }
コード例 #4
0
        private void ProcessReverse()
        {
            double planningDistance = GetPlanningDistance();

            // update the rndf path
            RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
            LinePath          curRndfPath  = rndfPath.Transform(relTransfrom);

            Console.WriteLine("cur rndf path count: " + curRndfPath.Count + ", " + curRndfPath.PathLength);
            Console.WriteLine("cur rndf path zero point valid: " + curRndfPath.ZeroPoint.Valid + ", loc: " + curRndfPath.ZeroPoint.Location + ", index: " + curRndfPath.ZeroPoint.Index);
            Console.WriteLine("planning dist: " + planningDistance + ", stop dist: " + GetSpeedCommandStopDistance());

            // get the path in reverse
            double   dist       = -(planningDistance + TahoeParams.RL + 2);
            LinePath targetPath = curRndfPath.SubPath(curRndfPath.ZeroPoint, ref dist);

            if (dist < 0)
            {
                targetPath.Add(curRndfPath[0] - curRndfPath.GetSegment(0).Vector.Normalize(-dist));
            }

            Console.WriteLine("target path count: " + targetPath.Count + ", " + targetPath.PathLength);
            Console.WriteLine("target path zero point valud: " + targetPath.ZeroPoint.Valid);

            // generate a box by shifting the path
            LinePath leftBound  = targetPath.ShiftLateral(rndfPathWidth / 2.0);
            LinePath rightBound = targetPath.ShiftLateral(-rndfPathWidth / 2.0);

            double leftStartDist, rightStartDist;

            GetLaneBoundStartDists(targetPath, rndfPathWidth, out leftStartDist, out rightStartDist);
            leftBound  = leftBound.RemoveBefore(leftBound.AdvancePoint(leftBound.StartPoint, leftStartDist));
            rightBound = rightBound.RemoveBefore(rightBound.AdvancePoint(rightBound.StartPoint, rightStartDist));

            AddTargetPath(targetPath, 0.0025);
            AddLeftBound(leftBound, false);
            AddRightBound(rightBound, false);

            avoidanceBasePath = targetPath;
            double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);

            smootherBasePath = targetPath.SubPath(targetPath.StartPoint, targetDist);

            settings.maxSpeed            = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
            settings.endingPositionFixed = true;
            settings.endingPositionMax   = rndfPathWidth / 2.0;
            settings.endingPositionMin   = -rndfPathWidth / 2.0;
            settings.Options.reverse     = true;

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

            SmoothAndTrack(commandLabel, true);
        }
コード例 #5
0
        private void ProcessReverse()
        {
            double planningDistance = reverseDist - Services.TrackedDistance.GetDistanceTravelled(reverseTimestamp, curTimestamp);

            // update the rndf path
            AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);

            // get the vehicle relative path
            LinePath relRecommendedPath = recommendedPath.Transform(absTransform);
            LinePath targetPath;

            if (relRecommendedPath.ZeroPoint.Location.Length > 10)
            {
                targetPath = new LinePath();
                targetPath.Add(new Coordinates(0, 0));
                targetPath.Add(new Coordinates(-(planningDistance + TahoeParams.RL + 2), 0));
            }
            else
            {
                // get the path in reverse
                double dist = -(planningDistance + TahoeParams.RL + 2);
                targetPath = relRecommendedPath.SubPath(relRecommendedPath.ZeroPoint, ref dist);
                if (dist < 0)
                {
                    targetPath.Add(relRecommendedPath[0] - relRecommendedPath.GetSegment(0).Vector.Normalize(-dist));
                }
            }

            AddTargetPath(targetPath, 0.005);

            avoidanceBasePath = targetPath;
            double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);

            smootherBasePath = new LinePath();
            smootherBasePath.Add(Coordinates.Zero);
            smootherBasePath.Add(targetPath.AdvancePoint(targetPath.StartPoint, targetDist).Location);

            settings.maxSpeed        = recommendedSpeed.Speed;
            settings.Options.reverse = true;
            settings.Options.w_diff  = 3;
            laneWidthAtPathEnd       = 5;
            useAvoidancePath         = false;

            Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);

            SmoothAndTrack(reverse_label, true);
        }
コード例 #6
0
 private bool TestNormalModeClear(LinePath relativePath, LinePath.PointOnPath closestPoint)
 {
     if (arcModeTimer != null && arcModeTimer.ElapsedMilliseconds < 5000) return false;
     return Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) < 3 && Math.Abs(relativePath.GetSegment(closestPoint.Index).UnitVector.ArcTan) < 45*Math.PI/180.0;
 }
コード例 #7
0
        public static void SmoothAndTrack(LinePath basePath, bool useTargetPath, 
            IList<LinePath> leftBounds, IList<LinePath> rightBounds,
            double maxSpeed, double? endingHeading, bool endingOffsetBound,
            CarTimestamp curTimestamp, bool doAvoidance,
            SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
            string commandLabel, ref bool cancelled,
            ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double? approachSpeed)
        {
            // if we're in listen mode, just return for now
            if (OperationalBuilder.BuildMode == BuildMode.Listen) {
                return;
            }

            PathPlanner.PlanningResult result;

            double curSpeed = Services.StateProvider.GetVehicleState().speed;
            LinePath targetPath = new LinePath();
            double initialHeading = 0;
            // get the part we just used to make a prediction
            if (useTargetPath && previousSmoothedPath != null) {
                //targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
                // interpolate the path with a smoothing spline
                //targetPath = targetPath.SplineInterpolate(0.05);
                //Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
                // calculate the point speed*dt ahead
                /*double lookaheadDist = curSpeed*0.20;
                if (lookaheadDist > 0.1) {
                    LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
                    // get the heading
                    initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
                    // adjust the base path start point to be the predicted location
                    basePath[0] = pt.Location;

                    // get the part we just used to make a prediction
                    predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
                    // push to the UI
                    Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
                    //basePath[0] = new Coordinates(lookaheadDist, 0);
                    Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
                    Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
                    // calculate a piece of the sub path
                    //targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
                }*/

                // get the tracking manager to predict stuff like whoa
                AbsolutePose absPose;
                OperationalVehicleState vehicleState;
                Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
                // insert the stuff stuff
                basePath[0] = absPose.xy;
                initialHeading = absPose.heading;

                // start walking down the path until the angle is cool
                double angle_threshold = 30*Math.PI/180.0;
                double dist;
                LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
                for (dist = 0; dist < 10; dist += 1) {
                    // get the point advanced from the 2nd point on the base path by dist
                    double distTemp = dist;
                    newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);

                    // check if we're past the end
                    if (distTemp > 0) {
                        break;
                    }

                    // check if the angle is coolness or not
                    double angle = Math.Acos((newPoint.Location-basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));

                    if (Math.Acos(angle) < angle_threshold) {
                        break;
                    }
                }

                // create a new version of the base path with the stuff section removed
                basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);

                Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");

                // zero that stuff out
                targetPath = new LinePath();
            }

            StaticObstacles obstacles = null;
            // only do the planning is we're in a lane scenario
            // otherwise, the obstacle grid will be WAY too large
            if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1) {
                // get the obstacles predicted to the current timestamp
                obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
            }

            // start the planning timer
            Stopwatch planningTimer = Stopwatch.StartNew();

            // check if there are any obstacles
            if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0) {
                if (cancelled) return;

                // we need to do the full obstacle avoidance
                // execute the obstacle manager
                LinePath avoidancePath;
                List<ObstacleManager.ObstacleType> obstacleSideFlags;
                bool success;
                Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
                    out avoidancePath, out obstacleSideFlags, out success);

                // check if we have success
                if (success) {
                    // build the boundary lists
                    // start with the lanes
                    List<Boundary> leftSmootherBounds  = new List<Boundary>();
                    List<Boundary> rightSmootherBounds = new List<Boundary>();

                    double laneMinSpacing = 0.1;
                    double laneDesiredSpacing = 0.1;
                    double laneAlphaS = 0.1;
                    leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
                    rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));

                    // sort out obstacles as left and right
                    double obstacleMinSpacing = 0.8;
                    double obstacleDesiredSpacing = 0.8;
                    double obstacleAlphaS = 100;
                    int totalObstacleClusters = obstacles.polygons.Count;
                    for (int i = 0; i < totalObstacleClusters; i++) {
                        if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left) {
                            Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
                            bound.CheckFrontBumper = true;
                            leftSmootherBounds.Add(bound);
                        }
                        else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right) {
                            Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
                            bound.CheckFrontBumper = true;
                            rightSmootherBounds.Add(bound);
                        }
                    }

                    if (cancelled) return;

                    // execute the smoothing
                    PathPlanner planner = new PathPlanner();
                    planner.Options.alpha_w = 0;
                    planner.Options.alpha_d = 10;
                    planner.Options.alpha_c = 10;
                    result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
                }
                else {
                    // mark that we did not succeed
                    result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
                }
            }
            else {

                if (cancelled) return;

                // do the path smoothing
                PathPlanner planner = new PathPlanner();

                List<LineList> leftList = new List<LineList>();
                foreach (LinePath ll in leftBounds) leftList.Add(ll);
                List<LineList> rightList = new List<LineList>();
                foreach (LinePath rl in rightBounds) rightList.Add(rl);

                planner.Options.alpha_w = 0;
                planner.Options.alpha_s = 0.1;
                planner.Options.alpha_d = 10;
                planner.Options.alpha_c = 10;

                result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
            }

            planningTimer.Stop();

            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);

            Services.Dataset.ItemAs<bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);

            if (result.result == SmoothResult.Sucess) {
                // insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
                //Coordinates startingVec = result.path[1].Point-result.path[0].Point;
                //Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
                //result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));

                previousSmoothedPath = new LinePath(result.path);
                previousSmoothedPathTimestamp = curTimestamp;
                Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);

                if (cancelled) return;

                // we've planned out the path, now build up the command
                ISpeedGenerator speedGenerator;
                if (speedCommand is ScalarSpeedCommand) {
                    /*if (result.path.HasSpeeds) {
                        speedGenerator = result.path;
                    }
                    else {*/
                    speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
                    //}
                }
                else if (speedCommand is StopAtDistSpeedCommand) {
                    StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
                    IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
                    speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());

                }
                else if (speedCommand is StopAtLineSpeedCommand) {
                    IDistanceProvider distProvider = new StoplineDistanceProvider();
                    speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
                }
                else if (speedCommand == null) {
                    throw new InvalidOperationException("Speed command is null");
                }
                else {
                    throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
                }

                if (cancelled) return;

                // build up the command
                TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
                trackingCommand.Label = commandLabel;

                // queue it to execute
                Services.TrackingManager.QueueCommand(trackingCommand);
            }
        }
コード例 #8
0
        public DirectionAlong DirectionAlongSegment(LinePath lp)
        {
            // get heading of the lane path there
            Coordinates pathVector = lp.GetSegment(0).UnitVector;

            // get vehicle heading
            Coordinates unit = new Coordinates(1, 0);
            Coordinates headingVector = unit.Rotate(this.Heading.ArcTan);

            // rotate vehicle heading
            Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan);

            // get path heading
            double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees());

            if (relativeVehicleDegrees < 70)
                return DirectionAlong.Forwards;
            else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110)
                return DirectionAlong.Perpendicular;
            else
                return DirectionAlong.Reverse;
        }
コード例 #9
0
        public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
                                          IList <LinePath> leftBounds, IList <LinePath> rightBounds,
                                          double maxSpeed, double?endingHeading, bool endingOffsetBound,
                                          CarTimestamp curTimestamp, bool doAvoidance,
                                          SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
                                          string commandLabel, ref bool cancelled,
                                          ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double?approachSpeed)
        {
            // if we're in listen mode, just return for now
            if (OperationalBuilder.BuildMode == BuildMode.Listen)
            {
                return;
            }

            PathPlanner.PlanningResult result;

            double   curSpeed       = Services.StateProvider.GetVehicleState().speed;
            LinePath targetPath     = new LinePath();
            double   initialHeading = 0;

            // get the part we just used to make a prediction
            if (useTargetPath && previousSmoothedPath != null)
            {
                //targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
                // interpolate the path with a smoothing spline
                //targetPath = targetPath.SplineInterpolate(0.05);
                //Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
                // calculate the point speed*dt ahead

                /*double lookaheadDist = curSpeed*0.20;
                 * if (lookaheadDist > 0.1) {
                 *      LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
                 *      // get the heading
                 *      initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
                 *      // adjust the base path start point to be the predicted location
                 *      basePath[0] = pt.Location;
                 *
                 *      // get the part we just used to make a prediction
                 *      predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
                 *      // push to the UI
                 *      Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
                 *      //basePath[0] = new Coordinates(lookaheadDist, 0);
                 *      Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
                 *      Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
                 *      // calculate a piece of the sub path
                 *      //targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
                 * }*/

                // get the tracking manager to predict stuff like whoa
                AbsolutePose            absPose;
                OperationalVehicleState vehicleState;
                Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
                // insert the stuff stuff
                basePath[0]    = absPose.xy;
                initialHeading = absPose.heading;

                // start walking down the path until the angle is cool
                double angle_threshold = 30 * Math.PI / 180.0;
                double dist;
                LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
                for (dist = 0; dist < 10; dist += 1)
                {
                    // get the point advanced from the 2nd point on the base path by dist
                    double distTemp = dist;
                    newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);

                    // check if we're past the end
                    if (distTemp > 0)
                    {
                        break;
                    }

                    // check if the angle is coolness or not
                    double angle = Math.Acos((newPoint.Location - basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));

                    if (Math.Acos(angle) < angle_threshold)
                    {
                        break;
                    }
                }

                // create a new version of the base path with the stuff section removed
                basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);

                Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");

                // zero that stuff out
                targetPath = new LinePath();
            }

            StaticObstacles obstacles = null;

            // only do the planning is we're in a lane scenario
            // otherwise, the obstacle grid will be WAY too large
            if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1)
            {
                // get the obstacles predicted to the current timestamp
                obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
            }

            // start the planning timer
            Stopwatch planningTimer = Stopwatch.StartNew();

            // check if there are any obstacles
            if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0)
            {
                if (cancelled)
                {
                    return;
                }

                // we need to do the full obstacle avoidance
                // execute the obstacle manager
                LinePath avoidancePath;
                List <ObstacleManager.ObstacleType> obstacleSideFlags;
                bool success;
                Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
                                                          out avoidancePath, out obstacleSideFlags, out success);

                // check if we have success
                if (success)
                {
                    // build the boundary lists
                    // start with the lanes
                    List <Boundary> leftSmootherBounds  = new List <Boundary>();
                    List <Boundary> rightSmootherBounds = new List <Boundary>();

                    double laneMinSpacing     = 0.1;
                    double laneDesiredSpacing = 0.1;
                    double laneAlphaS         = 0.1;
                    leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
                    rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));

                    // sort out obstacles as left and right
                    double obstacleMinSpacing     = 0.8;
                    double obstacleDesiredSpacing = 0.8;
                    double obstacleAlphaS         = 100;
                    int    totalObstacleClusters  = obstacles.polygons.Count;
                    for (int i = 0; i < totalObstacleClusters; i++)
                    {
                        if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left)
                        {
                            Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
                            bound.CheckFrontBumper = true;
                            leftSmootherBounds.Add(bound);
                        }
                        else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right)
                        {
                            Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
                            bound.CheckFrontBumper = true;
                            rightSmootherBounds.Add(bound);
                        }
                    }

                    if (cancelled)
                    {
                        return;
                    }

                    // execute the smoothing
                    PathPlanner planner = new PathPlanner();
                    planner.Options.alpha_w = 0;
                    planner.Options.alpha_d = 10;
                    planner.Options.alpha_c = 10;
                    result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
                }
                else
                {
                    // mark that we did not succeed
                    result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
                }
            }
            else
            {
                if (cancelled)
                {
                    return;
                }

                // do the path smoothing
                PathPlanner planner = new PathPlanner();

                List <LineList> leftList = new List <LineList>();
                foreach (LinePath ll in leftBounds)
                {
                    leftList.Add(ll);
                }
                List <LineList> rightList = new List <LineList>();
                foreach (LinePath rl in rightBounds)
                {
                    rightList.Add(rl);
                }

                planner.Options.alpha_w = 0;
                planner.Options.alpha_s = 0.1;
                planner.Options.alpha_d = 10;
                planner.Options.alpha_c = 10;

                result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
            }

            planningTimer.Stop();

            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);

            Services.Dataset.ItemAs <bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);

            if (result.result == SmoothResult.Sucess)
            {
                // insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
                //Coordinates startingVec = result.path[1].Point-result.path[0].Point;
                //Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
                //result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));

                previousSmoothedPath          = new LinePath(result.path);
                previousSmoothedPathTimestamp = curTimestamp;
                Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);

                if (cancelled)
                {
                    return;
                }

                // we've planned out the path, now build up the command
                ISpeedGenerator speedGenerator;
                if (speedCommand is ScalarSpeedCommand)
                {
                    /*if (result.path.HasSpeeds) {
                     *      speedGenerator = result.path;
                     * }
                     * else {*/
                    speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
                    //}
                }
                else if (speedCommand is StopAtDistSpeedCommand)
                {
                    StopAtDistSpeedCommand stopCommand  = (StopAtDistSpeedCommand)speedCommand;
                    IDistanceProvider      distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
                    speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
                }
                else if (speedCommand is StopAtLineSpeedCommand)
                {
                    IDistanceProvider distProvider = new StoplineDistanceProvider();
                    speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
                }
                else if (speedCommand == null)
                {
                    throw new InvalidOperationException("Speed command is null");
                }
                else
                {
                    throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
                }

                if (cancelled)
                {
                    return;
                }

                // build up the command
                TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
                trackingCommand.Label = commandLabel;

                // queue it to execute
                Services.TrackingManager.QueueCommand(trackingCommand);
            }
        }
コード例 #10
0
        private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
        {
            // construct the result object to hold stuff
            LaneModelTestResult result = new LaneModelTestResult();

            // project the lane model's path into the rndf path's timestamp
            RelativeTransform relTransform  = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
            LinePath          laneModelPath = laneModel.LanePath.Transform(relTransform);

            // get the zero point of the lane model path
            LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;

            // get the first waypoint on the RNDF path
            LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;

            // get the heading of the rndf path at its zero point and the heading of the lane model at
            // the rndf's zero point
            LinePath.PointOnPath laneStartPoint   = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
            Coordinates          laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
            Coordinates          rndfHeading      = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
            double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));

            // check if the angle is within limits for comparing offset
            if (angle < 30 * Math.PI / 180.0)
            {
                // get the deviation between lane zero point and rndf zero point
                result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
            }

            // now start check for how many waypoints are accepted
            for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
            {
                // check the distance along the rndf path
                double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
                // break out if we're too far along the rndf
                if (rndfDistAlong > 50)
                {
                    break;
                }

                // get the waypoint
                Coordinates waypoint = rndfPath[i];

                // project on to lane path
                LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);

                // check if we're too far along the lane path
                double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
                if (distAlong > lane_model_max_dist || distAlong < 0)
                {
                    break;
                }

                // check if the deviation
                double dist = waypoint.DistanceTo(laneWaypoint.Location);

                // increment appropriate counts
                if (dist < lane_deviation_reject_threshold)
                {
                    result.forward_match_count++;
                }
                else
                {
                    result.forward_rejection_count++;
                }
            }

            // return the result
            return(result);
        }
コード例 #11
0
        private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
        {
            // construct the result object to hold stuff
            LaneModelTestResult result = new LaneModelTestResult();

            // project the lane model's path into the rndf path's timestamp
            RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
            LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);

            // get the zero point of the lane model path
            LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;

            // get the first waypoint on the RNDF path
            LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;

            // get the heading of the rndf path at its zero point and the heading of the lane model at
            // the rndf's zero point
            LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
            Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
            Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
            double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));

            // check if the angle is within limits for comparing offset
            if (angle < 30*Math.PI/180.0) {
                // get the deviation between lane zero point and rndf zero point
                result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
            }

            // now start check for how many waypoints are accepted
            for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) {
                // check the distance along the rndf path
                double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
                // break out if we're too far along the rndf
                if (rndfDistAlong > 50) {
                    break;
                }

                // get the waypoint
                Coordinates waypoint = rndfPath[i];

                // project on to lane path
                LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);

                // check if we're too far along the lane path
                double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
                if (distAlong > lane_model_max_dist || distAlong < 0) {
                    break;
                }

                // check if the deviation
                double dist = waypoint.DistanceTo(laneWaypoint.Location);

                // increment appropriate counts
                if (dist < lane_deviation_reject_threshold) {
                    result.forward_match_count++;
                }
                else {
                    result.forward_rejection_count++;
                }
            }

            // return the result
            return result;
        }
コード例 #12
0
 private bool TestNormalModeClear(LinePath relativePath, LinePath.PointOnPath closestPoint)
 {
     if (arcModeTimer != null && arcModeTimer.ElapsedMilliseconds < 5000)
     {
         return(false);
     }
     return(Math.Abs(closestPoint.OfftrackDistance(Coordinates.Zero)) < 3 && Math.Abs(relativePath.GetSegment(closestPoint.Index).UnitVector.ArcTan) < 45 * Math.PI / 180.0);
 }
コード例 #13
0
        protected void GetIntersectionPullPath(LinePath startingPath, LinePath endingPath, Polygon intersectionPolygon, bool addStartingPoint, bool addEndingPoint, LinePath targetPath, ref double pullWeight)
        {
            double angle = Math.Acos(startingPath.EndSegment.UnitVector.Dot(endingPath.GetSegment(0).UnitVector));

            // get the centroid of the intersection
            Coordinates centroid;

            // check if the angle is great than an threshold
            if (angle > 10*Math.PI/180.0) {
                // intersect the two lines formed by the starting and ending lanes
                Line startingLaneLine = new Line(startingPath[startingPath.Count-2], startingPath[startingPath.Count-1]);
                Line endingLaneLine = new Line(endingPath[1], endingPath[0]);

                // intersect them stuff and see if the point of intersection is between the two lines
                Coordinates K;
                if (!startingLaneLine.Intersect(endingLaneLine, out centroid, out K) || K.X <= 0 || K.Y <= 0)
                    return;
            }
            else {
                // if there is no intersection polygon, there isn't much we can do
                if (intersectionPolygon == null || intersectionPolygon.Count < 3) {
                    return;
                }

                centroid = intersectionPolygon.GetCentroid();
            }

            // calculate the pull weighting dependent on angle of intersection
            // angle 0 -> 0 weighting
            // angle 45 -> 0.00025 weighting
            // angle 90 -> 0.001 weighting
            pullWeight = Math.Pow(angle/(Math.PI/2), 2)*0.001;

            // get the relative transform from the behavior timestamp to the current timestamp
            RelativeTransform transform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
            centroid = transform.TransformPoint(centroid);

            if (addStartingPoint) {
                targetPath.Add(startingPath.EndPoint.Location);
            }
            // add the line from exit -> centroid (assuming that exit is already in the target path)
            targetPath.Add(centroid);
            if (addEndingPoint) {
                // add the line from centroid -> entrance
                targetPath.Add(endingPath[0]);
            }

            Services.UIService.PushLineList(targetPath, curTimestamp, "intersection path", true);
            Services.Dataset.ItemAs<double>("intersection weight").Add(pullWeight, curTimestamp);
        }
コード例 #14
0
        private LinePath ReplaceFirstPoint(LinePath path, LinePath.PointOnPath maxAdvancePoint, Coordinates pt)
        {
            LinePath ret;
            if (!disablePathAngleCheck) {
                // start walking down the path until the angle is cool
                double angle_threshold = pathAngleMax;
                LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
                for (double dist = 0; dist < pathAngleCheckMax; dist += 1) {
                    // get the point advanced from the 2nd point on the base path by dist
                    double distTemp = dist;
                    newPoint = path.AdvancePoint(path.GetPointOnPath(1), ref distTemp);

                    // check if we're past the end
                    if (distTemp > 0) {
                        break;
                    }
                    else if (maxAdvancePoint.Valid && newPoint >= maxAdvancePoint) {
                        newPoint = maxAdvancePoint;
                        break;
                    }

                    // check if the angle is coolness or not
                    double angle = Math.Acos((newPoint.Location-pt).Normalize().Dot(path.GetSegment(newPoint.Index).UnitVector));

                    if (angle < angle_threshold) {
                        break;
                    }
                }

                // create a new version of the base path with the stuff section removed
                ret = path.RemoveBetween(path.StartPoint, newPoint);
                ret[0] = pt;
            }
            else {
                ret = path.Clone();
                ret[0] = pt;
            }

            return ret;
        }