public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ) { Polygon vehiclePoly = va.GetAbsolutePolygon(ourState); vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly); List <Coordinates> pointsOutside = new List <Coordinates>(); ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition); foreach (Coordinates c in vehiclePoly) { if (!p.IsInside(c)) { pointsOutside.Add(c); } } foreach (Coordinates m in pointsOutside) { foreach (Coordinates n in pointsOutside) { if (!m.Equals(n)) { if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) * GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0) { return(false); } } } } return(true); }
/// <summary> /// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane /// </summary> /// <param name="closestGood"></param> /// <param name="coordinates"></param> /// <param name="ignorable"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStopType"></param> /// <param name="navStop"></param> private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance, out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop) { ArbiterWaypoint current = null; double minDist = Double.MaxValue; StopType st = StopType.EndOfLane; #region Closest Good Parameterization foreach (ArbiterWaypoint aw in closestGood.WaypointList) { if (aw.IsStop || aw.NextPartition == null) { double dist = closestGood.DistanceBetween(coordinates, aw.Position); if (dist < minDist && dist >= 0) { current = aw; minDist = dist; st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane; } } } #endregion #region Opposing Parameterization ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial; int startIndex = opposing.WaypointList.IndexOf(opStart); for (int i = startIndex; i >= 0; i--) { ArbiterWaypoint aw = opposing.WaypointList[i]; if (aw.IsStop || aw.PreviousPartition == null) { double dist = opposing.DistanceBetween(aw.Position, coordinates); if (dist < minDist && dist >= 0) { current = aw; minDist = dist; st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane; } } } #endregion double tmpDistanceIgnore; this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore); navStop = current; navStopDistance = minDist; navStopType = st; }
public bool VehiclePassableInPolygon(ArbiterLane al, Polygon p, VehicleState ourState, Polygon circ) { List <Coordinates> vhcCoords = new List <Coordinates>(); for (int i = 0; i < this.trackedCluster.relativePoints.Length; i++) { vhcCoords.Add(this.TransformCoordAbs(this.trackedCluster.relativePoints[i], ourState)); } Polygon vehiclePoly = Polygon.GrahamScan(vhcCoords); vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly); ArbiterLanePartition alp = al.GetClosestPartition(this.trackedCluster.closestPoint); List <Coordinates> pointsOutside = new List <Coordinates>(); foreach (Coordinates c in vehiclePoly) { if (!p.IsInside(c)) { pointsOutside.Add(c); } } foreach (Coordinates m in pointsOutside) { foreach (Coordinates n in pointsOutside) { if (!m.Equals(n)) { if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) * GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0) { return(false); } } } } return(true); }
/// <summary> /// Startup the vehicle from a certain position, pass the next state back, /// and initialize the lane agent if possible /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public IState Startup(VehicleState vehicleState, CarMode carMode) { // check car mode if (carMode == CarMode.Run) { // check areas ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front); ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front); ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front); ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading); if (az != null) { // special zone startup return(new ZoneStartupState(az)); } if (ain != null) { if (al != null) { // check lane stuff PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front); // get distance from front of car double dist = lanePoint.pt.DistanceTo(vehicleState.Front); // check dist to lane if (dist < al.Width + 1.0) { // check orientation relative to lane Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize(); // get our heading Coordinates ourHeading = vehicleState.Heading.Normalize(); // forwards or backwards bool forwards = true; // check dist to each other if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0)) { // not going forwards forwards = false; } // stay in lane if forwards if (forwards) { ArbiterOutput.Output("Starting up in lane: " + al.ToString()); return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState)); } else { // Opposing lane return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState)); } } } // startup intersection state return(new IntersectionStartupState(ain)); } if (al != null) { // get a startup chute ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState); // check if in a startup chute if (startupChute != null && !startupChute.IsInside(vehicleState.Front)) { ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString()); return(new StartupOffChuteState(startupChute)); } // not in a startup chute else { PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front); // get distance from front of car double dist = lanePoint.pt.DistanceTo(vehicleState.Front); // check orientation relative to lane Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize(); // get our heading Coordinates ourHeading = vehicleState.Heading.Normalize(); // forwards or backwards bool forwards = true; // check dist to each other if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0)) { // not going forwards forwards = false; } // stay in lane if forwards if (forwards) { ArbiterOutput.Output("Starting up in lane: " + al.ToString()); return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState)); } else { // opposing return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState)); } } } // fell out ArbiterOutput.Output("Cannot find area to startup in"); return(CoreCommon.CorePlanningState); } else { return(CoreCommon.CorePlanningState); } }
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); }
private void HandleBehavior(StayInLaneBehavior b) { AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(b.TimeStamp); this.behaviorTimestamp = absTransform.Timestamp; this.rndfPath = b.BackupPath.Transform(absTransform); this.rndfPathWidth = b.LaneWidth; this.numLanesLeft = b.NumLaneLeft; this.numLanesRight = b.NumLanesRight; this.laneID = b.TargetLane; this.ignorableObstacles = b.IgnorableObstacles; Services.ObstaclePipeline.LastIgnoredObstacles = b.IgnorableObstacles; HandleSpeedCommand(b.SpeedCommand); opposingLaneVehicleExists = false; oncomingVehicleExists = false; extraWidth = 0; if (b.Decorators != null) { foreach (BehaviorDecorator d in b.Decorators) { if (d is OpposingLaneDecorator) { opposingLaneVehicleExists = true; opposingLaneVehicleDist = ((OpposingLaneDecorator)d).Distance; opposingLaneVehicleSpeed = ((OpposingLaneDecorator)d).Speed; } else if (d is OncomingVehicleDecorator) { oncomingVehicleExists = true; oncomingVehicleDist = ((OncomingVehicleDecorator)d).TargetDistance; oncomingVehicleSpeed = ((OncomingVehicleDecorator)d).TargetSpeed; oncomingVehicleSpeedCommand = ((OncomingVehicleDecorator)d).SecondarySpeed; } else if (d is WidenBoundariesDecorator) { extraWidth = ((WidenBoundariesDecorator)d).ExtraWidth; } } } if (oncomingVehicleExists) { double timeToCollision = oncomingVehicleDist / Math.Abs(oncomingVehicleSpeed); if (oncomingVehicleDist > 30 || timeToCollision > 20 || numLanesRight > 0) { oncomingVehicleExists = false; } else { HandleSpeedCommand(oncomingVehicleSpeedCommand); } } if (laneID != null && Services.RoadNetwork != null) { ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID]; AbsolutePose pose = Services.StateProvider.GetAbsolutePose(); sparse = (lane.GetClosestPartition(pose.xy).Type == PartitionType.Sparse); if (sparse) { LinePath.PointOnPath closest = b.BackupPath.GetClosestPoint(pose.xy); goalPoint = b.BackupPath[closest.Index + 1]; } } Services.UIService.PushAbsolutePath(b.BackupPath, b.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(new LineList(), b.TimeStamp, "original path2"); if (sparse) { if (Services.ObstaclePipeline.ExtraSpacing == 0) { Services.ObstaclePipeline.ExtraSpacing = 0.5; } Services.ObstaclePipeline.UseOccupancyGrid = false; } else { Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; smootherSpacingAdjust = 0; prevCurvature = double.NaN; } }