public void Initialize(Behavior b) { SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b; // store the base path basePath = ConvertPath(sb.BasePath); // get the lower, upper bound leftBound = FindBoundary(sb.LaneWidth / 2); rightBound = FindBoundary(-sb.LaneWidth / 2); // convert everything to be vehicle relative AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); pathTime = absTransform.Timestamp; basePath = basePath.Transform(absTransform); leftBound = leftBound.Transform(absTransform); rightBound = rightBound.Transform(absTransform); // send the left and right bounds Services.UIService.PushRelativePath(leftBound, pathTime, "left bound"); Services.UIService.PushRelativePath(rightBound, pathTime, "right bound"); maxSpeed = sb.MaxSpeed; obstacleManager = new ObstacleManager(); // hik }
public void Initialize(Behavior b) { SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b; // store the base path basePath = ConvertPath(sb.BasePath); // get the lower, upper bound leftBound = FindBoundary(sb.LaneWidth/2); rightBound = FindBoundary(-sb.LaneWidth/2); // convert everything to be vehicle relative AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); pathTime = absTransform.Timestamp; basePath = basePath.Transform(absTransform); leftBound = leftBound.Transform(absTransform); rightBound = rightBound.Transform(absTransform); // send the left and right bounds Services.UIService.PushRelativePath(leftBound, pathTime, "left bound"); Services.UIService.PushRelativePath(rightBound, pathTime, "right bound"); maxSpeed = sb.MaxSpeed; obstacleManager = new ObstacleManager(); // hik }
public LinePath LinearizeCenterLine(LinearizationOptions options) { LinePath transformedPath = centerlinePath; if (options.Timestamp.IsValid) { RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp); OperationalTrace.WriteVerbose("in LinearizeCenterLine, tried to find {0}->{1}, got {2}->{3}", timestamp, options.Timestamp, relTransform.OriginTimestamp, relTransform.EndTimestamp); transformedPath = centerlinePath.Transform(relTransform); } LinePath.PointOnPath startPoint = transformedPath.AdvancePoint(centerlinePath.ZeroPoint, options.StartDistance); LinePath subPath = new LinePath();; if (options.EndDistance > options.StartDistance) { subPath = transformedPath.SubPath(startPoint, options.EndDistance - options.StartDistance); } if (subPath.Count < 2) { subPath.Clear(); Coordinates startPt = startPoint.Location; subPath.Add(startPt); subPath.Add(centerlinePath.GetSegment(startPoint.Index).UnitVector *Math.Max(options.EndDistance - options.StartDistance, 0.1) + startPt); } return(subPath); }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; // extract the relevant information TurnBehavior cb = (TurnBehavior)b; targetLaneID = cb.TargetLane; // create a fake start lane so we can do the intersection pull stuff pseudoStartLane = new LinePath(); pseudoStartLane.Add(new Coordinates(-1, 0)); pseudoStartLane.Add(Coordinates.Zero); AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); pseudoStartLane = pseudoStartLane.Transform(absTransform.Invert()); HandleTurnBehavior(cb); curTimestamp = cb.TimeStamp; // do an initial plan without obstacle avoidance DoInitialPlan(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "turn behavior - initialized"); }
public LinePath LinearizeRightBound(LinearizationOptions options) { LinePath bound = rightBound; if (options.Timestamp.IsValid) { RelativeTransform relTransform = Services.RelativePose.GetTransform(timestamp, options.Timestamp); bound = bound.Transform(relTransform); } if (options.EndDistance > options.StartDistance) { // TODO: work off centerline path LinePath.PointOnPath startPoint = bound.AdvancePoint(bound.ZeroPoint, options.StartDistance); return(bound.SubPath(startPoint, options.EndDistance - options.StartDistance).ShiftLateral(options.LaneShiftDistance)); } else { return(new LinePath()); } }
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; } }
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 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()); } }
public override void Process(object param) { // inform the base that we're beginning processing if (!BeginProcess()) { return; } // check if we were given a parameter if (param != null && param is StayInLaneBehavior) { HandleBehavior((StayInLaneBehavior)param); } if (reverseGear) { ProcessReverse(); return; } // figure out the planning distance double planningDistance = GetPlanningDistance(); if (sparse && planningDistance > 10) { planningDistance = 10; } double?boundStartDistMin = null; double?boundEndDistMax = null; if (laneID != null && Services.RoadNetwork != null) { ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID]; AbsolutePose pose = Services.StateProvider.GetAbsolutePose(); ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy); LinePath partitionPath = partition.UserPartitionPath; LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy); double remainingDist = planningDistance; double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint); remainingDist -= totalDist; if (sparse) { // walk ahead and determine where sparsity ends bool nonSparseFound = false; while (remainingDist > 0) { // get the next partition partition = partition.Final.NextPartition; if (partition == null) { break; } if (partition.Type != PartitionType.Sparse) { nonSparseFound = true; break; } else { double dist = partition.UserPartitionPath.PathLength; totalDist += dist; remainingDist -= dist; } } if (nonSparseFound) { boundStartDistMin = totalDist; } } else { // determine if there is a sparse segment upcoming bool sparseFound = false; while (remainingDist > 0) { partition = partition.Final.NextPartition; if (partition == null) { break; } if (partition.Type == PartitionType.Sparse) { sparseFound = true; break; } else { double dist = partition.Length; totalDist += dist; remainingDist -= dist; } } if (sparseFound) { boundEndDistMax = totalDist; sparse = true; } } } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance); // update the rndf path RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp); LinePath curRndfPath = rndfPath.Transform(relTransfrom); ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight); double avoidanceExtra = sparse ? 5 : 7.5; LinePath centerLine, leftBound, rightBound; if (boundEndDistMax != null || boundStartDistMin != null) { LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound); } else { LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count); // calculate time to collision of opposing obstacle if it exists LinePath targetLine = centerLine; double targetWeight = default_lane_alpha_w; if (sparse) { targetWeight = 0.00000025; } else if (oncomingVehicleExists) { double shiftDist = -(TahoeParams.T + 1); targetLine = centerLine.ShiftLateral(shiftDist); targetWeight = 0.01; } else if (opposingLaneVehicleExists) { double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed)); Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp); if (timeToCollision < 5) { // shift to the right double shiftDist = -TahoeParams.T / 2.0; targetLine = centerLine.ShiftLateral(shiftDist); } } // set up the planning AddTargetPath(targetLine, targetWeight); if (!sparse || boundStartDistMin != null || boundEndDistMax != null) { AddLeftBound(leftBound, true); if (!oncomingVehicleExists) { AddRightBound(rightBound, false); } } double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance); smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist); maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint; double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength; extraDist = Math.Min(extraDist, 5); if (extraDist > 0) { centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist)); } avoidanceBasePath = centerLine; Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true); Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true); Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true); // get the overall max speed looking forward from our current point settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay)); // get the max speed at the end point settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance)); useAvoidancePath = false; if (sparse) { // limit to 5 mph laneWidthAtPathEnd = 20; pathAngleCheckMax = 50; pathAngleMax = 5 * Math.PI / 180.0; settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352); maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2); //maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2); LinePath leftEdge = RoadEdge.GetLeftEdgeLine(); LinePath rightEdge = RoadEdge.GetRightEdgeLine(); if (leftEdge != null) { leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false)); } if (rightEdge != null) { rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false)); } PlanningResult result = new PlanningResult(); ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint); if (commandGenerator == null) { // we have a block, report dynamically infeasible result = OnDynamicallyInfeasible(null, null); } else { result.steeringCommandGenerator = commandGenerator; result.commandLabel = commandLabel; } Track(result, commandLabel); return; } else if (oncomingVehicleExists) { laneWidthAtPathEnd = 10; } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); disablePathAngleCheck = false; SmoothAndTrack(commandLabel, true); }
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); }