Beispiel #1
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);
        }