コード例 #1
0
        public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed)
        {
            // get the angles
            List<Pair<int, double>> angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index);

            foreach (Pair<int, double> angleValue in angles) {
                // calculate the desired speed to take the point
                double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left));
                // calculate the desired speed at the intersection
                double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2));
                // limit the desired speed to 2.5
                desiredSpeed = Math.Max(2.5, desiredSpeed);
                // calculate the speed we would be allowed to go right now
                double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist);
                // check if the desired max speed is lower
                if (desiredMaxSpeed < maxSpeed) {
                    maxSpeed = desiredMaxSpeed;
                }
            }
        }
コード例 #2
0
        public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed)
        {
            // get the angles
            List <Pair <int, double> > angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index);

            foreach (Pair <int, double> angleValue in angles)
            {
                // calculate the desired speed to take the point
                double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left));
                // calculate the desired speed at the intersection
                double desiredSpeed = 1.5 / (angleValue.Right / (Math.PI / 2));
                // limit the desired speed to 2.5
                desiredSpeed = Math.Max(2.5, desiredSpeed);
                // calculate the speed we would be allowed to go right now
                double desiredMaxSpeed = Math.Sqrt(desiredSpeed * desiredSpeed + 2 * target_decel * dist);
                // check if the desired max speed is lower
                if (desiredMaxSpeed < maxSpeed)
                {
                    maxSpeed = desiredMaxSpeed;
                }
            }
        }
コード例 #3
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);
            }
        }
コード例 #4
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);
            }
        }
コード例 #5
0
        private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
        {
            // check the lane model probability
            if (laneModel.Probability < lane_probability_reject_threshold)
            {
                // we're rejecting this, just return a path lane model
                return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth));
            }

            // 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);

            // iterate through the waypoints in the RNDF path and project onto the lane model
            // the first one that is over the threshold, we consider the waypoint before as a potential ending point
            LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
            // flag indicating if any of the waypoint tests failed because of the devation was too high
            bool anyDeviationTooHigh = false;
            // number of waypoints accepted
            int numWaypointsAccepted = 0;

            // get the vehicle's position on the rndf path
            LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;

            // get the vehicle's position on the lane model
            LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;

            // get the last point we want to consider on the lane model
            LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);

            // start walking forward through the waypoints on the rndf path
            // this loop will implicitly exit when we're past the end of the lane model as the waypoints
            //		will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
            //    end of the path)
            for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
            {
                // get the waypoint
                Coordinates rndfWaypoint = rndfPath[i];

                // get the closest point on the lane model
                LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);

                // compute the distance between the two
                double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);

                // if this is above the deviation threshold, leave the loop
                if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint)
                {
                    // if we're at the end of the lane model path, we don't want to consider this a rejection
                    if (laneModelClosestPoint < laneModelFarthestPoint)
                    {
                        // mark that at least on deviation was too high
                        anyDeviationTooHigh = true;
                    }
                    break;
                }

                // increment the number of waypoint accepted
                numWaypointsAccepted++;

                // update the end point of where we're valid as the local road model was OK up to this point
                laneModelDeviationEndPoint = laneModelClosestPoint;
            }

            // go through and figure out how far out the variance is within tolerance
            LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
            // walk forward from this point until the end of the lane mode path
            for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++)
            {
                // check if we're within the variance toleration
                if (laneModel.LaneYVariance[i] <= y_var_reject_threshold)
                {
                    // we are, update the point on path
                    laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
                }
                else
                {
                    // we are out of tolerance, break out of the loop
                    break;
                }
            }

            // now figure out everything out
            // determine waypoint rejection status
            WaypointRejectionResult waypointRejectionResult;

            if (laneModelDeviationEndPoint.Valid)
            {
                // if the point is valid, that we had at least one waypoint that was ok
                // check if any waypoints were rejected
                if (anyDeviationTooHigh)
                {
                    // some waypoint was ok, so we know that at least one waypoint was accepted
                    waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
                }
                else
                {
                    // no waypoint triggered a rejection, but at least one was good
                    waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
                }
            }
            else
            {
                // the point is not valid, so we either had no waypoints or we had all rejections
                if (anyDeviationTooHigh)
                {
                    // the first waypoint was rejected, so all are rejected
                    waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
                }
                else
                {
                    // the first waypoint (if any) was past the end of the lane model
                    waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
                }
            }

            // criteria for determining if this path is valid:
            //	- if some or all waypoints were accepted, than this is probably a good path
            //		- if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
            //	- if there were no waypoints, this is a potentially dangerous situation since we can't reject
            //    or confirm the local road model. for now, we'll assume that it is correct but this may need to change
            //    if we start handling intersections in this framework
            //  - if all waypoints were rejected, than we don't use the local road model
            //  - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
            //    the lane model was in tolerance

            // now build out the lane model
            ILaneModel finalLaneModel;

            // check if we rejected all waypoints or no lane model points satisified the variance threshold
            if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid)
            {
                // want to just use the path lane model
                finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
            }
            else
            {
                // we'll use the lane model
                // need to build up the center line as well as left and right bounds

                // to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
                // or by rejections) and then use the rndf lane after that.
                LinePath centerLine = new LinePath();

                // figure out the max distance
                // if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
                if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints)
                {
                    laneModelDeviationEndPoint = laneModelFarthestPoint;
                }

                // figure out the closer of the end points
                LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
                bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;

                // add the lane model to the center line
                centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));

                // create a list to hold the width expansion values
                List <double> widthValue = new List <double>();

                // make the width expansion values the width of the path plus the 1-sigma values
                for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++)
                {
                    widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
                }

                // now figure out how to add the rndf path
                // get the projection of the lane model end point on the rndf path
                LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);

                // if the closest point is past the end of rndf path, then we don't want to tack anything on
                if (rndfPathStartPoint != rndfPath.EndPoint)
                {
                    // get the last segment of the new center line
                    Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
                    // get the last point of the new center line
                    Coordinates laneModelEndLoc = laneModelEndPoint.Location;

                    // now figure out the distance to the next waypoint
                    LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();

                    // figure out if we're ending at a waypoint or not
                    if (endAtWaypoint)
                    {
                        rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1);

                        // if the distance from the start point to the next point is less than rndf_dist_min, then
                        // use the waypont after
                        double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);

                        if (dist < rndf_dist_min)
                        {
                            if (rndfPathStartPoint.Index < rndfPath.Count - 2)
                            {
                                rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
                            }
                            else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min)
                            {
                                rndfNextPoint = LinePath.PointOnPath.Invalid;
                            }
                            else
                            {
                                rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2);
                            }
                        }
                    }
                    else
                    {
                        // track the last angle we had
                        double lastAngle = double.NaN;

                        // walk down the rndf path until we find a valid point
                        for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step)
                        {
                            // advance from the start point by dist
                            double distTemp = dist;
                            rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);

                            // if the distTemp is > 0, then we're past the end of the path
                            if (distTemp > 0)
                            {
                                // if we're immediately past the end, we don't want to tack anything on
                                if (dist == rndf_dist_min)
                                {
                                    rndfNextPoint = LinePath.PointOnPath.Invalid;
                                }

                                break;
                            }

                            // check the angle made by the last segment of center line and the segment
                            // formed between the end point of the center line and this new point
                            double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize()));

                            // check if the angle satisfies the threshold or we're increasing the angle
                            if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle))
                            {
                                // break out of the loop, we're done searching
                                break;
                            }

                            lastAngle = angle;
                        }
                    }

                    // tack on the rndf starting at next point going to the end
                    if (rndfNextPoint.Valid)
                    {
                        LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
                        centerLine.AddRange(subPath);

                        // insert the lane model end point into the sub path
                        subPath.Insert(0, laneModelEndLoc);

                        // get the angles
                        List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1);

                        // add the width of the path inflated by the angles
                        for (int i = 0; i < angles.Count; i++)
                        {
                            // calculate the width expansion factor
                            // 90 deg, 3x width
                            // 45 deg, 1.5x width
                            // 0 deg, 1x width
                            double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1;

                            // add the width value
                            widthValue.Add(widthFactor * laneModel.Width / 2);
                        }

                        // add the final width
                        widthValue.Add(laneModel.Width / 2);
                    }

                    // set the rndf path start point to be the point we used
                    rndfPathStartPoint = rndfNextPoint;
                }

                // for now, calculate the left and right bounds the same way we do for the path lane model
                // TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
                LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
                // get the next shifts
                for (int i = 0; i < widthValue.Count; i++)
                {
                    widthValue[i] = -widthValue[i];
                }
                LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());

                // build the final lane model
                finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
            }

            SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);

            // output the fit result
            return(finalLaneModel);
        }
コード例 #6
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);
        }
        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;
        }
        private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
        {
            // check the lane model probability
            if (laneModel.Probability < lane_probability_reject_threshold) {
                // we're rejecting this, just return a path lane model
                return new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
            }

            // 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);

            // iterate through the waypoints in the RNDF path and project onto the lane model
            // the first one that is over the threshold, we consider the waypoint before as a potential ending point
            LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
            // flag indicating if any of the waypoint tests failed because of the devation was too high
            bool anyDeviationTooHigh = false;
            // number of waypoints accepted
            int numWaypointsAccepted = 0;

            // get the vehicle's position on the rndf path
            LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;

            // get the vehicle's position on the lane model
            LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;

            // get the last point we want to consider on the lane model
            LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);

            // start walking forward through the waypoints on the rndf path
            // this loop will implicitly exit when we're past the end of the lane model as the waypoints
            //		will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
            //    end of the path)
            for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) {
                // get the waypoint
                Coordinates rndfWaypoint = rndfPath[i];

                // get the closest point on the lane model
                LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);

                // compute the distance between the two
                double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);

                // if this is above the deviation threshold, leave the loop
                if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) {
                    // if we're at the end of the lane model path, we don't want to consider this a rejection
                    if (laneModelClosestPoint < laneModelFarthestPoint) {
                        // mark that at least on deviation was too high
                        anyDeviationTooHigh = true;
                    }
                    break;
                }

                // increment the number of waypoint accepted
                numWaypointsAccepted++;

                // update the end point of where we're valid as the local road model was OK up to this point
                laneModelDeviationEndPoint = laneModelClosestPoint;
            }

            // go through and figure out how far out the variance is within tolerance
            LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
            // walk forward from this point until the end of the lane mode path
            for (int i = laneModelZeroPoint.Index+1; i < laneModelPath.Count; i++) {
                // check if we're within the variance toleration
                if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) {
                    // we are, update the point on path
                    laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
                }
                else {
                    // we are out of tolerance, break out of the loop
                    break;
                }
            }

            // now figure out everything out
            // determine waypoint rejection status
            WaypointRejectionResult waypointRejectionResult;
            if (laneModelDeviationEndPoint.Valid) {
                // if the point is valid, that we had at least one waypoint that was ok
                // check if any waypoints were rejected
                if (anyDeviationTooHigh) {
                    // some waypoint was ok, so we know that at least one waypoint was accepted
                    waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
                }
                else {
                    // no waypoint triggered a rejection, but at least one was good
                    waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
                }
            }
            else {
                // the point is not valid, so we either had no waypoints or we had all rejections
                if (anyDeviationTooHigh) {
                    // the first waypoint was rejected, so all are rejected
                    waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
                }
                else {
                    // the first waypoint (if any) was past the end of the lane model
                    waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
                }
            }

            // criteria for determining if this path is valid:
            //	- if some or all waypoints were accepted, than this is probably a good path
            //		- if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
            //	- if there were no waypoints, this is a potentially dangerous situation since we can't reject
            //    or confirm the local road model. for now, we'll assume that it is correct but this may need to change
            //    if we start handling intersections in this framework
            //  - if all waypoints were rejected, than we don't use the local road model
            //  - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
            //    the lane model was in tolerance

            // now build out the lane model
            ILaneModel finalLaneModel;

            // check if we rejected all waypoints or no lane model points satisified the variance threshold
            if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) {
                // want to just use the path lane model
                finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
            }
            else {
                // we'll use the lane model
                // need to build up the center line as well as left and right bounds

                // to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
                // or by rejections) and then use the rndf lane after that.
                LinePath centerLine = new LinePath();

                // figure out the max distance
                // if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
                if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) {
                    laneModelDeviationEndPoint = laneModelFarthestPoint;
                }

                // figure out the closer of the end points
                LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
                bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;

                // add the lane model to the center line
                centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));

                // create a list to hold the width expansion values
                List<double> widthValue = new List<double>();

                // make the width expansion values the width of the path plus the 1-sigma values
                for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index+centerLine.Count; i++) {
                    widthValue.Add(laneModel.Width/2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
                }

                // now figure out how to add the rndf path
                // get the projection of the lane model end point on the rndf path
                LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);

                // if the closest point is past the end of rndf path, then we don't want to tack anything on
                if (rndfPathStartPoint != rndfPath.EndPoint) {
                    // get the last segment of the new center line
                    Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
                    // get the last point of the new center line
                    Coordinates laneModelEndLoc = laneModelEndPoint.Location;

                    // now figure out the distance to the next waypoint
                    LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();

                    // figure out if we're ending at a waypoint or not
                    if (endAtWaypoint) {
                        rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index+1);

                        // if the distance from the start point to the next point is less than rndf_dist_min, then
                        // use the waypont after
                        double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);

                        if (dist < rndf_dist_min) {
                            if (rndfPathStartPoint.Index < rndfPath.Count-2) {
                                rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
                            }
                            else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) {
                                rndfNextPoint = LinePath.PointOnPath.Invalid;
                            }
                            else {
                                rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min*2);
                            }
                        }
                    }
                    else {
                        // track the last angle we had
                        double lastAngle = double.NaN;

                        // walk down the rndf path until we find a valid point
                        for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) {
                            // advance from the start point by dist
                            double distTemp = dist;
                            rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);

                            // if the distTemp is > 0, then we're past the end of the path
                            if (distTemp > 0) {
                                // if we're immediately past the end, we don't want to tack anything on
                                if (dist == rndf_dist_min) {
                                    rndfNextPoint = LinePath.PointOnPath.Invalid;
                                }

                                break;
                            }

                            // check the angle made by the last segment of center line and the segment
                            // formed between the end point of the center line and this new point
                            double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location-laneModelEndLoc).Normalize()));

                            // check if the angle satisfies the threshold or we're increasing the angle
                            if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) {
                                // break out of the loop, we're done searching
                                break;
                            }

                            lastAngle = angle;
                        }
                    }

                    // tack on the rndf starting at next point going to the end
                    if (rndfNextPoint.Valid) {
                        LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
                        centerLine.AddRange(subPath);

                        // insert the lane model end point into the sub path
                        subPath.Insert(0, laneModelEndLoc);

                        // get the angles
                        List<Pair<int, double>> angles = subPath.GetIntersectionAngles(0, subPath.Count-1);

                        // add the width of the path inflated by the angles
                        for (int i = 0; i < angles.Count; i++) {
                            // calculate the width expansion factor
                            // 90 deg, 3x width
                            // 45 deg, 1.5x width
                            // 0 deg, 1x width
                            double widthFactor = Math.Pow(angles[i].Right/(Math.PI/2.0), 2)*2 + 1;

                            // add the width value
                            widthValue.Add(widthFactor*laneModel.Width/2);
                        }

                        // add the final width
                        widthValue.Add(laneModel.Width/2);
                    }

                    // set the rndf path start point to be the point we used
                    rndfPathStartPoint = rndfNextPoint;
                }

                // for now, calculate the left and right bounds the same way we do for the path lane model
                // TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
                LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
                // get the next shifts
                for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; }
                LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());

                // build the final lane model
                finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
            }

            SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);

            // output the fit result
            return finalLaneModel;
        }
コード例 #9
0
        protected virtual double GetMaxSpeed(LinePath rndfPath, LinePath.PointOnPath startingPoint)
        {
            double maxSpeed;
            if (speedCommand is ScalarSpeedCommand) {
                maxSpeed = Math.Min(((ScalarSpeedCommand)speedCommand).Speed, 15);

                // calculate the swerving max speed
                double deltaSteeringSigma = Services.TrackingManager.DeltaSteeringStdDev*180/Math.PI;

                if (deltaSteeringSigma > 3) {
                    double swerveMax = Math.Max(18 - 0.2*deltaSteeringSigma, 2.5);
                    maxSpeed = Math.Min(maxSpeed, swerveMax);
                }

                approachSpeed = null;
            }
            else {
                // assume that it's a stop at dist or stop at line
                // either way, we do the same thing
                if (approachSpeed == null || (double)approachSpeed < 1) {
                    approachSpeed = Math.Max(vs.speed, 1);
                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "approach speed set to {0}", approachSpeed.Value);
                }
                maxSpeed = approachSpeed.Value;
            }

            if (rndfPath != null) {
                // get the angles
                List<Pair<int, double>> angles = rndfPath.GetIntersectionAngles(startingPoint.Index, rndfPath.Count-1);

                foreach (Pair<int, double> angleValue in angles) {
                    // calculate the desired speed to take the point
                    double dist = Math.Max(rndfPath.DistanceBetween(startingPoint, rndfPath.GetPointOnPath(angleValue.Left))-TahoeParams.FL, 0);
                    // calculate the desired speed at the intersection
                    double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2));
                    // limit the desired speed to 2.5
                    desiredSpeed = Math.Max(2.5, desiredSpeed);
                    // calculate the speed we would be allowed to go right now
                    double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist);
                    // check if the desired max speed is lower
                    if (desiredMaxSpeed < maxSpeed) {
                        maxSpeed = desiredMaxSpeed;
                    }
                }
            }

            return maxSpeed;
        }
コード例 #10
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;
        }