public void Process(object param)
        {
            if (cancelled)
            {
                return;
            }

            DateTime start = HighResDateTime.Now;

            OperationalVehicleState vs = Services.StateProvider.GetVehicleState();

            LinePath curBasePath   = basePath;
            LinePath curLeftBound  = leftBound;
            LinePath curRightBound = rightBound;

            // transform the base path to the current iteration
            CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;

            if (pathTime != curTimestamp)
            {
                RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
                curBasePath   = curBasePath.Transform(relTransform);
                curLeftBound  = curLeftBound.Transform(relTransform);
                curRightBound = curRightBound.Transform(relTransform);
            }

            // get the distance between the zero point and the start point
            double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));

            // get the sub-path between 5 and 25 meters ahead
            double startDist = TahoeParams.FL;

            LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
            double endDist = 30;

            LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);

            if (startDist > 0)
            {
                // we've reached the end
                Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
                return;
            }

            // get the sub-path
            LinePath subPath = curBasePath.SubPath(startPoint, endPoint);

            // add (0,0) as the starting point
            subPath.Insert(0, new Coordinates(0, 0));

            // do the same for the left, right bound
            startDist    = TahoeParams.FL;
            endDist      = 40;
            startPoint   = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
            endPoint     = curLeftBound.AdvancePoint(startPoint, endDist);
            curLeftBound = curLeftBound.SubPath(startPoint, endPoint);

            startPoint    = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
            endPoint      = curRightBound.AdvancePoint(startPoint, endDist);
            curRightBound = curRightBound.SubPath(startPoint, endPoint);

            if (cancelled)
            {
                return;
            }

            Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
            Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
            Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");

            // run a path smoothing iteration
            lock (this) {
                planner = new PathPlanner();
            }

            ////////////////////////////////////////////////////////////////////////////////////////////////////
            // start of obstacle manager - hik
            bool obstacleManagerEnable = true;

            PathPlanner.SmoothingResult result;

            if (obstacleManagerEnable == true)
            {
                // generate fake obstacles (for simulation testing only)
                double             obsSize          = 10.5 / 2;
                List <Coordinates> obstaclePoints   = new List <Coordinates>();
                List <Obstacle>    obstacleClusters = new List <Obstacle>();
                // fake left obstacles (for simulation only)
                int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
                for (int i = 0; i < totalLeftObstacles; i++)
                {
                    obstaclePoints.Clear();
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
                    obstacleClusters.Add(new Obstacle());
                    obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
                }
                // fake right obstacles (for simulation only)
                int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
                for (int i = 0; i < totalRightObstacles; i++)
                {
                    obstaclePoints.Clear();
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
                    obstacleClusters.Add(new Obstacle());
                    obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
                }
                // fake center obstacles (for simulation only)
                int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
                for (int i = 2; i < totalCenterObstacles; i++)
                {
                    obstaclePoints.Clear();
                    obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
                    obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
                    obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
                    obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
                    obstacleClusters.Add(new Obstacle());
                    obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
                }

                obstaclePoints.Clear();
                obstaclePoints.Add(new Coordinates(10000, 10000));
                obstaclePoints.Add(new Coordinates(10000, 10001));
                obstaclePoints.Add(new Coordinates(10001, 10000));
                obstacleClusters.Add(new Obstacle());
                obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);

                obstaclePoints.Clear();
                obstaclePoints.Add(new Coordinates(1000, 1000));
                obstaclePoints.Add(new Coordinates(1000, 1001));
                obstaclePoints.Add(new Coordinates(1001, 1000));
                obstacleClusters.Add(new Obstacle());
                obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);

                obstaclePoints.Clear();
                obstaclePoints.Add(new Coordinates(-1000, -1000));
                obstaclePoints.Add(new Coordinates(-1000, -1001));
                obstaclePoints.Add(new Coordinates(-1001, -1000));
                obstacleClusters.Add(new Obstacle());
                obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);

                foreach (Obstacle obs in obstacleClusters)
                {
                    obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
                }

                // find obstacle path and left/right classification
                LinePath obstaclePath = new LinePath();
                //Boolean successFlag;
                //double laneWidthAtPathEnd = 10.0;
//#warning this currently doesn't work

                /*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
                 *                                     obstacleClusters, laneWidthAtPathEnd,
                 *                                                                                                                               out obstaclePath, out successFlag);
                 */

                // prepare left and right lane bounds
                double          laneMinSpacing     = 0.1;
                double          laneDesiredSpacing = 0.5;
                double          laneAlphaS         = 10000;
                List <Boundary> leftBounds         = new List <Boundary>();
                List <Boundary> rightBounds        = new List <Boundary>();
                leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
                rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));

                // sort out obstacles as left and right
                double   obstacleMinSpacing     = 0.1;
                double   obstacleDesiredSpacing = 1.0;
                double   obstacleAlphaS         = 10000;
                Boundary bound;
                int      totalObstacleClusters = obstacleClusters.Count;
                for (int i = 0; i < totalObstacleClusters; i++)
                {
                    if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left)
                    {
                        // obstacle cluster is to the left of obstacle path
                        bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
                                             obstacleDesiredSpacing, obstacleAlphaS, true);
                        bound.CheckFrontBumper = true;
                        leftBounds.Add(bound);
                    }
                    else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right)
                    {
                        // obstacle cluster is to the right of obstacle path
                        bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
                                             obstacleDesiredSpacing, obstacleAlphaS, true);
                        bound.CheckFrontBumper = true;
                        rightBounds.Add(bound);
                    }
                    else
                    {
                        // obstacle cluster is outside grid, hence ignore obstacle cluster
                    }
                }

                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();

                // PlanPath function call with obstacle path and obstacles
                result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
                                          0, maxSpeed, vs.speed, null, curTimestamp, false);

                stopwatch.Stop();
                Console.WriteLine("============================================================");
                Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
                Console.WriteLine("============================================================");
            }
            else
            {
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();

                // original PlanPath function call
                result = planner.PlanPath(subPath, curLeftBound, curRightBound,
                                          0, maxSpeed, vs.speed, null, curTimestamp);

                stopwatch.Stop();
                Console.WriteLine("============================================================");
                Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
                Console.WriteLine("============================================================");
            }

            // end of obstacle manager - hik
            ////////////////////////////////////////////////////////////////////////////////////////////////////

            //PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);

            //SmoothedPath path = new SmoothedPath(pathTime);

            lock (this) {
                planner = null;
            }

            if (cancelled)
            {
                return;
            }

            if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess)
            {
                // start tracking the path
                Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
                //Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));

                /*TrackingCommand cmd = new TrackingCommand(
                 *      new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
                 *      new SinSteeringCommandGenerator(),
                 *      true);
                 * Services.TrackingManager.QueueCommand(cmd);*/

                // send the path's we're tracking to the UI
                Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");

                cancelled = true;
            }
        }
Example #2
0
        public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
        {
            List <Coordinates> polyPoints = new List <Coordinates>();

            try
            {
                // width
                double width = 3.0;
                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }
                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }

                if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
                    ai.TurnDirection == ArbiterTurnDirection.Straight ||
                    !(ai.InitialGeneric is ArbiterWaypoint) ||
                    !(ai.FinalGeneric is ArbiterWaypoint))
                {
                    LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
                    LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
                    polyPoints.AddRange(lp);
                    polyPoints.AddRange(rp);
                    ai.TurnPolygon = Polygon.GrahamScan(polyPoints);

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        List <Coordinates> updatedPts = new List <Coordinates>();
                        LinePath           interTmp   = ai.InterconnectPath.Clone();
                        Coordinates        pathVec    = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                        interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
                        interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
                        lp          = interTmp.ShiftLateral(TahoeParams.VL);
                        rp          = interTmp.ShiftLateral(-TahoeParams.VL);
                        updatedPts.AddRange(lp);
                        updatedPts.AddRange(rp);
                        ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
                    }
                }
                else
                {
                    // polygon points
                    List <Coordinates> interPoints = new List <Coordinates>();

                    // waypoint
                    ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
                    ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;

                    // left and right path
                    LinePath leftPath  = new LinePath();
                    LinePath rightPath = new LinePath();

                    // some initial points
                    LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
                    LinePath il          = initialPath.ShiftLateral(width / 2.0);
                    LinePath ir          = initialPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(il[1]);
                    rightPath.Add(ir[1]);

                    // some final points
                    LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
                    LinePath fl        = finalPath.ShiftLateral(width / 2.0);
                    LinePath fr        = finalPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(fl[0]);
                    rightPath.Add(fr[0]);

                    // initial and final paths
                    Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
                    Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);

                    // get where the paths intersect and vector to normal path
                    Coordinates c;
                    iPath.Intersect(fPath, out c);
                    Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
                    Coordinates center = c + vector.Normalize((vector.Length / 2.0));

                    // get width expansion
                    Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);

                    double      iRot = -iVec.ArcTan;
                    Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);

                    fVec = fVec.Rotate(iRot);
                    double fDeg        = fVec.ToDegrees();
                    double arcTan      = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
                    double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;

                    // get inner point (small scale)
                    Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);

                    // get outer
                    Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);

                    if (ai.TurnDirection == ArbiterTurnDirection.Right)
                    {
                        rightPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = rightPath;
                        leftPath.Reverse();
                        leftPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                    else
                    {
                        leftPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = leftPath;
                        rightPath.Reverse();
                        rightPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("error generating turn polygon: " + ai.ToString());
                ai.TurnPolygon = ai.DefaultPoly();
            }
        }
Example #3
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);
        }
Example #4
0
        public override void Process(object param)
        {
            try {
                Trace.CorrelationManager.StartLogicalOperation("ChangeLanes");

                if (!base.BeginProcess())
                {
                    return;
                }

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

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "got new param -- speed {0}, dist {1}, dist timestamp {2}", clParam.SpeedCommand, clParam.MaxDist, clParam.TimeStamp);
                }

                // project the lane paths up to the current time
                RelativeTransform relTransform    = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
                LinePath          curStartingPath = startingLanePath.Transform(relTransform);
                LinePath          curEndingPath   = endingLanePath.Transform(relTransform);

                // get the starting and ending lane models
                ILaneModel startingLaneModel, endingLaneModel;
                Services.RoadModelProvider.GetLaneChangeModels(curStartingPath, startingLaneWidth, startingNumLanesLeft, startingNumLanesRight,
                                                               curEndingPath, endingLaneWidth, changeLeft, behaviorTimestamp, out startingLaneModel, out endingLaneModel);

                // calculate the max speed
                // TODO: make this look ahead for slowing down
                settings.maxSpeed = GetMaxSpeed(endingLanePath, endingLanePath.ZeroPoint);

                // get the remaining lane change distance
                double remainingDist = GetRemainingLaneChangeDistance();

                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "remaining distance is {0}", remainingDist);

                if (cancelled)
                {
                    return;
                }

                // check if we're done
                if (remainingDist <= 0)
                {
                    // create a new stay in lane behavior
                    int deltaLanes = changeLeft ? -1 : 1;
                    StayInLaneBehavior stayInLane = new StayInLaneBehavior(endingLaneID, speedCommand, null, endingLanePath, endingLaneWidth, startingNumLanesLeft + deltaLanes, startingNumLanesRight - deltaLanes);
                    stayInLane.TimeStamp = behaviorTimestamp.ts;
                    Services.BehaviorManager.Execute(stayInLane, null, false);

                    // send completion report
                    ForwardCompletionReport(new SuccessCompletionReport(typeof(ChangeLaneBehavior)));

                    return;
                }

                // calculate the planning distance
                double planningDist = GetPlanningDistance();
                if (planningDist < remainingDist + TahoeParams.VL)
                {
                    planningDist = remainingDist + TahoeParams.VL;
                }

                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning dist {0}", planningDist);

                // create a linearization options structure
                LinearizationOptions laneOpts = new LinearizationOptions();
                laneOpts.Timestamp = curTimestamp;

                // get the center line of the target lane starting at the distance we want to enter into it
                laneOpts.StartDistance = remainingDist;
                laneOpts.EndDistance   = planningDist;
                LinePath centerLine = endingLaneModel.LinearizeCenterLine(laneOpts);

                // add the final center line as a weighting
                AddTargetPath(centerLine.Clone(), default_lane_alpha_w);

                // pre-pend (0,0) as our position
                centerLine.Insert(0, new Coordinates(0, 0));

                LinePath leftBound  = null;
                LinePath rightBound = null;
                // figure out if the lane is to the right or left of us
                laneOpts.EndDistance = planningDist;
                double leftEndingStartDist, rightEndingStartDist;
                GetLaneBoundStartDists(curEndingPath, endingLaneWidth, out leftEndingStartDist, out rightEndingStartDist);
                if (changeLeft)
                {
                    // we're the target lane is to the left
                    // get the left bound of the target lane

                    laneOpts.StartDistance = Math.Max(leftEndingStartDist, TahoeParams.FL);
                    leftBound = endingLaneModel.LinearizeLeftBound(laneOpts);
                }
                else
                {
                    // we're changing to the right, get the right bound of the target lane
                    laneOpts.StartDistance = Math.Max(rightEndingStartDist, TahoeParams.FL);
                    rightBound             = endingLaneModel.LinearizeRightBound(laneOpts);
                }

                // get the other bound as the starting lane up to 5m before the remaining dist
                laneOpts.StartDistance = TahoeParams.FL;
                laneOpts.EndDistance   = Math.Max(0, remainingDist - 5);
                if (changeLeft)
                {
                    if (laneOpts.EndDistance > 0)
                    {
                        rightBound = startingLaneModel.LinearizeRightBound(laneOpts);
                    }
                    else
                    {
                        rightBound = new LinePath();
                    }
                }
                else
                {
                    if (laneOpts.EndDistance > 0)
                    {
                        leftBound = startingLaneModel.LinearizeLeftBound(laneOpts);
                    }
                    else
                    {
                        leftBound = new LinePath();
                    }
                }

                // append on the that bound of the target lane starting at the remaining dist
                laneOpts.StartDistance = Math.Max(remainingDist, TahoeParams.FL);
                laneOpts.EndDistance   = planningDist;
                if (changeLeft)
                {
                    rightBound.AddRange(endingLaneModel.LinearizeRightBound(laneOpts));
                }
                else
                {
                    leftBound.AddRange(endingLaneModel.LinearizeLeftBound(laneOpts));
                }

                // set up the planning
                smootherBasePath = centerLine;
                AddLeftBound(leftBound, !changeLeft);
                AddRightBound(rightBound, changeLeft);

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

                if (cancelled)
                {
                    return;
                }

                // set auxillary options
                settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan;

                // smooth and track that stuff
                SmoothAndTrack(commandLabel, true);
            }
            finally {
                Trace.CorrelationManager.StopLogicalOperation();
            }
        }
        public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
        {
            List<Coordinates> polyPoints = new List<Coordinates>();
            try
            {
                // width
                double width = 3.0;
                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }
                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }

                if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
                    ai.TurnDirection == ArbiterTurnDirection.Straight ||
                    !(ai.InitialGeneric is ArbiterWaypoint) ||
                    !(ai.FinalGeneric is ArbiterWaypoint))
                {
                    LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
                    LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
                    polyPoints.AddRange(lp);
                    polyPoints.AddRange(rp);
                    ai.TurnPolygon = Polygon.GrahamScan(polyPoints);

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        List<Coordinates> updatedPts = new List<Coordinates>();
                        LinePath interTmp = ai.InterconnectPath.Clone();
                        Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                        interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
                        interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
                        lp = interTmp.ShiftLateral(TahoeParams.VL);
                        rp = interTmp.ShiftLateral(-TahoeParams.VL);
                        updatedPts.AddRange(lp);
                        updatedPts.AddRange(rp);
                        ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
                    }
                }
                else
                {
                    // polygon points
                    List<Coordinates> interPoints = new List<Coordinates>();

                    // waypoint
                    ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
                    ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;

                    // left and right path
                    LinePath leftPath = new LinePath();
                    LinePath rightPath = new LinePath();

                    // some initial points
                    LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
                    LinePath il = initialPath.ShiftLateral(width / 2.0);
                    LinePath ir = initialPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(il[1]);
                    rightPath.Add(ir[1]);

                    // some final points
                    LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
                    LinePath fl = finalPath.ShiftLateral(width / 2.0);
                    LinePath fr = finalPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(fl[0]);
                    rightPath.Add(fr[0]);

                    // initial and final paths
                    Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
                    Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);

                    // get where the paths intersect and vector to normal path
                    Coordinates c;
                    iPath.Intersect(fPath, out c);
                    Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
                    Coordinates center = c + vector.Normalize((vector.Length / 2.0));

                    // get width expansion
                    Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
                    double iRot = -iVec.ArcTan;
                    Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    double fDeg = fVec.ToDegrees();
                    double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
                    double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;

                    // get inner point (small scale)
                    Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);

                    // get outer
                    Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);

                    if (ai.TurnDirection == ArbiterTurnDirection.Right)
                    {
                        rightPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = rightPath;
                        leftPath.Reverse();
                        leftPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                    else
                    {
                        leftPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = leftPath;
                        rightPath.Reverse();
                        rightPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("error generating turn polygon: " + ai.ToString());
                ai.TurnPolygon = ai.DefaultPoly();
            }
        }
        public override void Process(object param)
        {
            if (!base.BeginProcess())
            {
                return;
            }

            // check if we're given a param
            if (param != null && param is TurnBehavior)
            {
                TurnBehavior turnParam = (TurnBehavior)param;

                HandleTurnBehavior(turnParam);

                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "turn behavior - got new param, speed command {0}", turnParam.SpeedCommand);
            }

            LinePath curTargetPath = targetPath.Clone();
            LinePath curLeftBound  = null;

            if (leftBound != null)
            {
                curLeftBound = leftBound.Clone();
            }
            LinePath curRightBound = null;

            if (rightBound != null)
            {
                curRightBound = rightBound.Clone();
            }
            // transform the path into the current timestamp
            if (behaviorTimestamp != curTimestamp)
            {
                // get the relative transform
                RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, transforming from {0}->{1}, wanted {2}->{3}", relTransform.OriginTimestamp, relTransform.EndTimestamp, behaviorTimestamp, curTimestamp);
                curTargetPath.TransformInPlace(relTransform);
                if (curLeftBound != null)
                {
                    curLeftBound.TransformInPlace(relTransform);
                }
                if (curRightBound != null)
                {
                    curRightBound.TransformInPlace(relTransform);
                }
            }

            // get the distance between our current location and the start point
            double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);

            double startDist = Math.Max(0, TahoeParams.FL - distToStart);

            // extract off the first 5 m of the target path
            LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, TahoeParams.VL);
            curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);
            curTargetPath = curTargetPath.RemoveZeroLengthSegments();

            // adjust the left bound and right bounds starting distance
            if (curLeftBound != null)
            {
                LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
                curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
                AddLeftBound(curLeftBound, false);
                Services.UIService.PushLineList(curLeftBound, curTimestamp, "left bound", true);
            }

            if (curRightBound != null)
            {
                LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
                curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
                AddRightBound(curRightBound, false);

                Services.UIService.PushLineList(curRightBound, curTimestamp, "right bound", true);
            }

            if (cancelled)
            {
                return;
            }

            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, dist to start: {0}", distToStart);
            if (distToStart < TahoeParams.FL * 0.75)
            {
                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in turn behavior, past start point of target path");
                // return a completion report
                ForwardCompletionReport(new SuccessCompletionReport(typeof(TurnBehavior)));
            }

            AddTargetPath(curTargetPath.Clone(), 0.005);

            // transform the pseudo start lane to the current timestamp
            AbsoluteTransformer trans        = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
            LinePath            curStartPath = pseudoStartLane.Transform(trans);

            // add the intersection pull path
            LinePath intersectionPullPath = new LinePath();
            double   pullWeight           = 0;

            GetIntersectionPullPath(curStartPath, curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);

            if (intersectionPullPath.Count > 0)
            {
                AddTargetPath(intersectionPullPath, pullWeight);
            }

            // set up planning details
            // add our position to the current target path
            LinePath origTargetPath = curTargetPath.Clone();

            curTargetPath.Insert(0, new Coordinates(0, 0));
            //curTargetPath.Insert(1, new Coordinates(1, 0));

            smootherBasePath = curTargetPath;
            // add the bounds

            // calculate max speed
            settings.maxSpeed       = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
            settings.Options.w_diff = 4;

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

            if (cancelled)
            {
                return;
            }

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

            // set the avoidance path to the previously planned path
            // transform to the current timestamp
            disablePathAngleCheck = true;
            if (turnBasePath != null)
            {
                avoidanceBasePath = turnBasePath.Transform(trans);
                avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.GetClosestPoint(Coordinates.Zero), avoidanceBasePath.EndPoint);
                maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.GetPointOnPath(1);

                if (avoidanceBasePath.PathLength > 7.5)
                {
                    smootherBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.StartPoint, avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -7.5));
                }
                else
                {
                    smootherBasePath = avoidanceBasePath.Clone();
                }

                disablePathAngleCheck = false;
            }

            // fill in auxiliary settings
            if (curLeftBound != null || curRightBound != null)
            {
                settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
            }

            settings.endingPositionFixed = false;
            settings.endingPositionMax   = 16;
            settings.endingPositionMin   = -16;
            if (curLeftBound != null && curLeftBound.Count > 0 && curTargetPath.Count > 0)
            {
                double leftWidth = curLeftBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
                settings.endingPositionMax   = leftWidth;
                settings.endingPositionFixed = true;
            }

            if (curRightBound != null && curRightBound.Count > 0 && curTargetPath.Count > 0)
            {
                double rightWidth = curRightBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2;
                settings.endingPositionFixed = true;
                settings.endingPositionMin   = -rightWidth;
            }

            //disablePathAngleCheck = true;
            //useAvoidancePath = true;

            // do the planning
            SmoothAndTrack(commandLabel, true);
        }
        private void DoInitialPlan()
        {
            InitializePlanningSettings();

            curTimestamp = Services.RelativePose.CurrentTimestamp;

            vs = Services.StateProvider.GetVehicleState();

            LinePath curTargetPath = targetPath.Clone();
            LinePath curLeftBound  = null;

            if (leftBound != null)
            {
                curLeftBound = leftBound.Clone();
            }
            LinePath curRightBound = null;

            if (rightBound != null)
            {
                curRightBound = rightBound.Clone();
            }

            // get the distance between our current location and the start point
            double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]);

            // extract off the first 5 m of the target path
            LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, 12);
            curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget);

            AddTargetPath(curTargetPath.Clone(), 0.005);

            // adjust the left bound and right bounds starting distance
            if (curLeftBound != null)
            {
                LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2);
                curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint);
                AddLeftBound(curLeftBound, false);
            }
            if (curRightBound != null)
            {
                LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2);
                curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint);
                AddRightBound(curRightBound, false);
            }

            // add the intersection pull path
            LinePath            intersectionPullPath = new LinePath();
            double              pullWeight           = 0;
            AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);

            GetIntersectionPullPath(pseudoStartLane.Transform(trans), curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight);

            if (intersectionPullPath.Count > 0)
            {
                AddTargetPath(intersectionPullPath, pullWeight);
            }

            // set up planning details
            // add our position to the current target path
            curTargetPath.Insert(0, new Coordinates(0, 0));
            smootherBasePath = curTargetPath;
            // add the bounds

            // calculate max speed
            settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);

            // fill in auxiliary settings
            if (curLeftBound != null || curRightBound != null)
            {
                settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan;
            }
            disablePathAngleCheck = true;

            settings.Options.w_diff = 4;

            // do the planning
            PlanningResult planningResult = Smooth(false);

            // if the planning was a success, store the result
            if (!planningResult.dynamicallyInfeasible)
            {
                // transform to absolute coordinates
                AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);
                turnBasePath = planningResult.smoothedPath.Transform(absTransform.Invert());
            }
        }
Example #8
0
        public override void Process(object param)
        {
            if (!base.BeginProcess())
            {
                return;
            }

            if (param is ZoneTravelingBehavior)
            {
                HandleBaseBehavior((ZoneTravelingBehavior)param);
            }

            extraObstacles = GetPerimeterObstacles();

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

            AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp);

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

            LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint;

            // get the distance to the end point
            double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint);

            // get the planning distance
            double planningDist = GetPlanningDistance();

            planningDist  = Math.Max(planningDist, 20);
            planningDist -= zeroPoint.Location.Length;
            if (planningDist < 2.5)
            {
                planningDist = 2.5;
            }

            if (distToEnd < planningDist)
            {
                // make the speed command at stop speed command
                behaviorTimestamp = curTimestamp;
                speedCommand      = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL);
                planningDist      = distToEnd;
                approachSpeed     = recommendedSpeed.Speed;

                settings.endingHeading       = relRecommendedPath.EndSegment.UnitVector.ArcTan;
                settings.endingPositionFixed = true;
                settings.endingPositionMax   = 2;
                settings.endingPositionMin   = -2;
            }
            else
            {
                speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed);
            }

            // get the distance of the path segment we care about
            LinePath pathSegment   = relRecommendedPath.SubPath(zeroPoint, planningDist);
            double   avoidanceDist = planningDist + 5;

            avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist);
            if (avoidanceDist > 0)
            {
                avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist));
            }

            // test if we should clear out of arc mode
            if (arcMode)
            {
                if (TestNormalModeClear(relRecommendedPath, zeroPoint))
                {
                    prevCurvature = double.NaN;
                    arcMode       = false;
                }
            }

            if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1)
            {
                pathSegment.Insert(0, Coordinates.Zero);
            }
            else
            {
                if (pathSegment[0].DistanceTo(pathSegment[1]) < 1)
                {
                    pathSegment.RemoveAt(0);
                }
                pathSegment[0] = Coordinates.Zero;
            }

            if (arcMode)
            {
                Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location;
                ArcVoteZone(relativeGoalPoint, extraObstacles);
                return;
            }

            double pathLength = pathSegment.PathLength;

            if (pathLength < 6)
            {
                double additionalDist = 6.25 - pathLength;
                pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist));
            }

            // determine if polygons are to the left or right of the path
            for (int i = 0; i < zoneBadRegions.Length; i++)
            {
                Polygon poly = zoneBadRegions[i].Transform(absTransform);

                int numLeft  = 0;
                int numRight = 0;
                foreach (LineSegment ls in pathSegment.GetSegmentEnumerator())
                {
                    for (int j = 0; j < poly.Count; j++)
                    {
                        if (ls.IsToLeft(poly[j]))
                        {
                            numLeft++;
                        }
                        else
                        {
                            numRight++;
                        }
                    }
                }

                if (numLeft > numRight)
                {
                    // we'll consider this polygon on the left of the path
                    //additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
                }
                else
                {
                    //additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0));
                }
            }

            // TODO: add zone perimeter
            disablePathAngleCheck   = false;
            laneWidthAtPathEnd      = 7;
            settings.Options.w_diff = 3;
            smootherBasePath        = new LinePath();
            smootherBasePath.Add(Coordinates.Zero);
            smootherBasePath.Add(pathSegment.EndPoint.Location);
            AddTargetPath(pathSegment, 0.005);

            settings.maxSpeed = recommendedSpeed.Speed;
            useAvoidancePath  = false;

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

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

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

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

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

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

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

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

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

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

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

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

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

            // remove the first FL distance of the center line
            if (laneStartDist > 0) {
                centerLine = centerLine.RemoveBefore(centerLine.AdvancePoint(centerLine.ZeroPoint, laneStartDist));
                centerLine.Insert(0, Coordinates.Zero);
            }
        }