/// <summary> /// Constructor /// </summary> /// <param name="zonePerimeter">Perimeter of hte zone</param> /// <param name="stayOutside">Polygons to stay outside of</param> /// <param name="speedCommand">Recommended Speed command</param> public ZoneBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand recommendedSpeedCommand) { this.zoneId = zoneId; this.zonePerimeter = zonePerimeter; this.stayOutside = stayOutside; this.speedCommand = recommendedSpeedCommand; }
/// <summary> /// Constructor /// </summary> /// <param name="zonePerimeter">Perimeter of hte zone</param> /// <param name="stayOutside">Polygons to stay outside of</param> /// <param name="speedCommand">Recommended Speed command</param> public ZoneBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand recommendedSpeedCommand) { this.zoneId = zoneId; this.zonePerimeter = zonePerimeter; this.stayOutside = stayOutside; this.speedCommand = recommendedSpeedCommand; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="recommendedPath"></param> /// <param name="endingLeftBound"></param> /// <param name="endingRightBound"></param> public ZoneTravelingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath recommendedPath, LinePath endingLeftBound, LinePath endingRightBound) : base(zoneId, zonePerimeter, stayOutside, speedCommand) { this.RecommendedPath = recommendedPath; this.EndingLeftBound = endingLeftBound; this.EndingRightBound = endingRightBound; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="parkingSpotPath"></param> /// <param name="spotLeftBound"></param> /// <param name="spotRightBound"></param> /// <param name="spotId"></param> /// <param name="extraDistance"></param> /// <param name="endingPolygon"></param> /// <param name="orientations"></param> public ZoneParkingPullOutBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, LinePath reversePath, LinePath finalLeftBound, LinePath finalRightBound) : base(zoneId, zonePerimeter, stayOutside, speedCommand, parkingSpotPath, spotLeftBound, spotRightBound, spotId, 0.0) { this.RecommendedPullOutPath = reversePath; this.EndingLeftBound = finalLeftBound; this.EndingRightBound = finalRightBound; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="parkingSpotPath"></param> /// <param name="spotLeftBound"></param> /// <param name="spotRightBound"></param> /// <param name="spotId"></param> /// <param name="extraDistance"></param> public ZoneParkingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, double extraDistance) : base(zoneId, zonePerimeter, stayOutside, speedCommand) { this.ParkingSpotPath = parkingSpotPath; this.SpotLeftBound = spotLeftBound; this.SpotRightBound = spotRightBound; this.ParkingSpotId = spotId; this.ExtraDistance = extraDistance; }
protected void HandleBaseBehavior(ZoneBehavior b) { // storing everything in absolute coordinates this.zonePerimeter = b.ZonePerimeter; this.zoneBadRegions = b.StayOutside; this.recommendedSpeed = b.RecommendedSpeed; Services.UIService.PushPolygons(b.StayOutside, b.TimeStamp, "zone bad regions", false); Services.UIService.PushPolygon(b.ZonePerimeter, b.TimeStamp, "zone perimeter", false); }
/// <summary> /// Gets parameterization /// </summary> /// <param name="lane"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state) { double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance; Maneuver m = new Maneuver(); SpeedCommand sc; bool usingSpeed = true; #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff) { // default behavior sc = new StopAtDistSpeedCommand(distance); Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp); } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // get lane ArbiterLane al = lane; // default behavior sc = new ScalarSpeedCommand(speed); Behavior b = new StayInLaneBehavior(al.LaneId, sc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Plan a lane change /// </summary> /// <param name="cls"></param> /// <param name="initialManeuver"></param> /// <param name="targetManeuver"></param> /// <returns></returns> public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable) { // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // go to a blockage handling tactical return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // lanes of the lane change ArbiterLane initial = cls.Parameters.Initial; ArbiterLane target = cls.Parameters.Target; #region Initial Forwards if (!cls.Parameters.InitialOncoming) { ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial); #region Target Forwards if (!cls.Parameters.TargetOncoming) { // target reasoning ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target); #region Navigation if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // vehicles to ignore List<int> ignorableVehicles = new List<int>(); // params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle); ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3")); if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { tps.Add(initialParams); } else tps.Add(initialReasoning.ForwardMonitor.NavigationParameters); ignorableVehicles.AddRange(initialParams.VehiclesToIgnore); // get params for the final lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>()); TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters; tps.Add(targetParams); ignorableVehicles.AddRange(targetParams.VehiclesToIgnore); try { if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && targetParams.Type == TravellingType.Vehicle && targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null && targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed) { return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } catch (Exception) { } ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3")); // decorators List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // check if need to modify distance to go if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound; double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0; if (curDistToUpper > newVhcDistToUpper) { distanceToGo = newVhcDistToUpper; } } // get final tps.Sort(); // get the proper speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed); if (sc.Speed < 8.84) sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84)); // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion #region Failed Forwards else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // vehicles to ignore List<int> ignorableVehicles = new List<int>(); // params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null); tps.Add(initialParams); ignorableVehicles.AddRange(initialParams.VehiclesToIgnore); // get params for the final lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>()); TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters; tps.Add(targetParams); ignorableVehicles.AddRange(targetParams.VehiclesToIgnore); // decorators List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed); // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion #region Slow else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } #endregion else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion #region Target Oncoming else { OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target); #region Failed Forward if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // ignore the forward vehicle but keep params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null); tps.Add(initialParams); // get params for the final lane targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages); TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value; tps.Add(targetParams); // decorators List<BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24)); // check final for stopped failed opposing VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle; if (forwardVa != null) { // dist between double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition); // check stopped bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5; // check distance bool distOk = distToFV < 2.5 * TahoeParams.VL; // check failed bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed; // notify ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString()); // check all for failed if (stopped && distOk && failed) { // check inside target if (target.LanePolygon.IsInside(vehicleState.Front)) { // blockage recovery StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState); StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false)); BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE, new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None), BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } // check which lane we are in else { // return to forward lane return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion #region Other else if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // fallout exception throw new Exception("currently unsupported lane change type"); } else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } else { // fallout exception throw new Exception("currently unsupported lane change type"); } #endregion } #endregion } #endregion #region Initial Oncoming else { OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial); #region Target Forwards if (!cls.Parameters.TargetOncoming) { ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target); if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } #region Navigation else if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // distance to the upper bound of the change double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get params for the initial lane initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages); // current params of the fqm TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value; if (initialParams.Type == TravellingType.Vehicle) { if(!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) tps.Add(initialParams); else { tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters); distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL; } } else tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters); // get params for forward lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable); TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? target.WaypointList[target.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle); tps.Add(targetParams); // ignoring vehicles add List<int> ignoreVehicles = initialParams.VehiclesToIgnore; ignoreVehicles.AddRange(targetParams.VehiclesToIgnore); // decorators List<BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = tps[0].SpeedCommand; if (sc is StopAtDistSpeedCommand) { sc = new ScalarSpeedCommand(0.0); } // check final for stopped failed opposing VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle; if (forwardVa != null) { // dist between double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition); // check stopped bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5; // check distance bool distOk = distToFV < 2.5 * TahoeParams.VL; // check failed bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed; // notify ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString()); // check all for failed if (stopped && distOk && failed) { // check which lane we are in if (initial.LanePolygon.IsInside(vehicleState.Front)) { // return to opposing lane return new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { // lane state return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo, sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion }
public OncomingVehicleDecorator(ScalarSpeedCommand secondarySpeed, double targetDistance, double targetSpeed) { this.secondarySpeed = secondarySpeed; this.targetDistance = targetDistance; this.targetSpeed = targetSpeed; }
/// <summary> /// Parameters to follow the forward vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters Follow(IFQMPlanable lane, VehicleState state, List<ArbiterWaypoint> ignorable) { // travelling parameters TravelingParameters tp = new TravelingParameters(); // get control parameters ForwardVehicleTrackingControl fvtc = GetControl(lane, state, ignorable); this.ForwardControl = fvtc; // initialize the parameters tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.Decorators = new List<BehaviorDecorator>(); // ignore the forward vehicles tp.VehiclesToIgnore = this.VehiclesToIgnore; #region Following Control #region Immediate Stop // need to stop immediately if (fvtc.vFollowing == 0.0) { // speed command SpeedCommand sc = new ScalarSpeedCommand(0.0); tp.SpeedCommand = sc; tp.UsingSpeed = true; if (lane is ArbiterLane) { // standard path following behavior ArbiterLane al = ((ArbiterLane)lane); Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); final.Decorators = tp.Decorators; tp.Behavior = final; } else { SupraLane sl = (SupraLane)lane; StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore); final.Decorators = tp.Decorators; tp.Behavior = final; } } #endregion #region Stopping at Distance // stop at distance else if (fvtc.vFollowing < 0.7 && CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 && fvtc.xSeparation > fvtc.xAbsMin) { // speed command SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood); tp.SpeedCommand = sc; tp.UsingSpeed = false; if (lane is ArbiterLane) { ArbiterLane al = (ArbiterLane)lane; // standard path following behavior Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); final.Decorators = tp.Decorators; tp.Behavior = final; } else { SupraLane sl = (SupraLane)lane; StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore); final.Decorators = tp.Decorators; tp.Behavior = final; } } #endregion #region Normal Following // else normal else { // speed command SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing); tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.UsingSpeed = true; tp.SpeedCommand = sc; if (lane is ArbiterLane) { ArbiterLane al = ((ArbiterLane)lane); // standard path following behavior Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); final.Decorators = tp.Decorators; tp.Behavior = final; } else { SupraLane sl = (SupraLane)lane; StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore); final.Decorators = tp.Decorators; tp.Behavior = final; } } #endregion #endregion #region Check for Oncoming Vehicles // check if need to add current lane oncoming vehicle decorator if (false && this.CurrentVehicle.PassedDelayedBirth && fvtc.forwardOncoming && fvtc.xSeparation > TahoeParams.VL && fvtc.xSeparation < 30) { // check valid lane area if (lane is ArbiterLane || ((SupraLane)lane).ClosestComponent(this.CurrentVehicle.ClosestPosition) == SLComponentType.Initial) { // get distance to and speed of the forward vehicle double fvDistance = fvtc.xSeparation; double fvSpeed = fvtc.vTarget; // create the 5mph behavior ScalarSpeedCommand updated = new ScalarSpeedCommand(2.24); // set that we are using speed tp.UsingSpeed = true; tp.RecommendedSpeed = updated.Speed; tp.DistanceToGo = fvtc.xSeparation; // create the decorator OncomingVehicleDecorator ovd = new OncomingVehicleDecorator(updated, fvDistance, fvSpeed); // add the decorator tp.Behavior.Decorators.Add(ovd); tp.Decorators.Add(ovd); } } #endregion // set current this.followingParameters = tp; // return parameterization return tp; }
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); }
/// <summary> /// Plans over the zone /// </summary> /// <param name="planningState"></param> /// <param name="navigationalPlan"></param> /// <param name="vehicleState"></param> /// <param name="vehicles"></param> /// <param name="obstacles"></param> /// <param name="blockages"></param> /// <returns></returns> public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List<ITacticalBlockage> blockages) { #region Zone Travelling State if (planningState is ZoneTravelingState) { // check blockages /*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // go to a blockage handling tactical return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp); }*/ // cast state ZoneState zs = (ZoneState)planningState; // plan over state and zone ZonePlan zp = (ZonePlan)navigationalPlan; // check zone path does not exist if (zp.RecommendedPath.Count < 2) { // zone startup again ZoneStartupState zss = new ZoneStartupState(zs.Zone, true); return new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // final path seg LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL); LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint); LinePath lB = lp.ShiftLateral(TahoeParams.T); LinePath rB = lp.ShiftLateral(-TahoeParams.T); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); // get speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24); // Behavior Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(), sc, zp.RecommendedPath, lB, rB); // maneuver return new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Parking State else if (planningState is ParkingState) { // get state ParkingState ps = (ParkingState)planningState; // determine stay out areas to use List<Polygon> stayOuts = new List<Polygon>(); foreach (Polygon p in ps.Zone.StayOutAreas) { if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position)) stayOuts.Add(p); } LinePath rB = ps.ParkingSpot.GetRightBound(); LinePath lB = ps.ParkingSpot.GetLeftBound(); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); // create behavior ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24), ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0); // maneuver return new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Pulling Out State else if (planningState is PullingOutState) { // get state PullingOutState pos = (PullingOutState)planningState; // plan over state and zone ZonePlan zp = (ZonePlan)navigationalPlan; // final path seg Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1]; Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0); LinePath rp = new LinePath(new Coordinates[]{pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position, zp.RecommendedPath[0], endBack}); LinePath lp = new LinePath(new Coordinates[]{zp.RecommendedPath[0], endBack}); LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0); LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound)); // determine stay out areas to use List<Polygon> stayOuts = new List<Polygon>(); foreach (Polygon p in pos.Zone.StayOutAreas) { if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position)) stayOuts.Add(p); } // get speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24); // Behavior Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId, rp, lB, rB); // maneuver return new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Zone Startup State else if (planningState is ZoneStartupState) { // state ZoneStartupState zss = (ZoneStartupState)planningState; // get the zone ArbiterZone az = zss.Zone; // navigational edge INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]; // check over all the parking spaces foreach (ArbiterParkingSpot aps in az.ParkingSpots) { // inside both parts of spot if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) || (vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position))) { // want to just park in it again return new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } Polygon forwardPolygon = vehicleState.ForwardPolygon; Polygon rearPolygon = vehicleState.RearPolygon; Navigator nav = CoreCommon.Navigation; List<ZoneNavigationEdgeSort> forwardForward = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> reverseReverse = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> perpendicularPerpendicular = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> allEdges = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> allEdgesNoLimits = new List<ZoneNavigationEdgeSort>(); foreach (NavigableEdge ne in az.NavigableEdges) { if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint)) { // get distance to edge LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position }); double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); // get direction along segment DirectionAlong da = vehicleState.DirectionAlongSegment(lp); // check dist reasonable if (distTmp > TahoeParams.VL) { // zone navigation edge sort item ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp); // add to lists if (da == DirectionAlong.Forwards && (forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position))) forwardForward.Add(znes); /*else if (da == DirectionAlong.Perpendicular && !(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) && !(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))) perpendicularPerpendicular.Add(znes); else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)) reverseReverse.Add(znes);*/ // add to all edges allEdges.Add(znes); } } } // sort by distance forwardForward.Sort(); reverseReverse.Sort(); perpendicularPerpendicular.Sort(); allEdges.Sort(); ZoneNavigationEdgeSort bestZnes = null; for (int i = 0; i < allEdges.Count; i++) { // get line to initial Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position); Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position); bool intersects = false; Coordinates[] interPts; foreach (Polygon sop in az.StayOutAreas) { if (!intersects && (sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts))) intersects = true; } if (!intersects) { allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn); allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24; ZoneNavigationEdgeSort tmpZnes = allEdges[i]; if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) || (bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time)) bestZnes = tmpZnes; } if (i > allEdges.Count / 2 && bestZnes != null) break; } if (bestZnes != null) { ArbiterOutput.Output("Found good edge to start in zone"); return new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked"); List<ZoneNavigationEdgeSort> okZnes = new List<ZoneNavigationEdgeSort>(); foreach (NavigableEdge tmpOk in az.NavigableEdges) { // get line to initial LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position }); double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath); tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn); tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24; if (tmpZnes.zp.RecommendedPath.Count >= 2) okZnes.Add(tmpZnes); } if (okZnes.Count == 0) okZnes = allEdges; else okZnes.Sort(); // get random close edge Random rand = new Random(); int chosen = rand.Next(Math.Min(5, okZnes.Count)); // get closest edge not part of a parking spot, get on it NavigableEdge closest = okZnes[chosen].edge;//null; //double distance = Double.MaxValue; /*foreach (NavigableEdge ne in az.NavigableEdges) { if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint)) { // get distance to edge LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position }); double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); if (closest == null || distTmp < distance) { closest = ne; distance = distTmp; } } }*/ return new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion #region Unknown else { // non-handled state throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState"); } #endregion }
/// <summary> /// Generates a default turn behavior /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <param name="turn"></param> /// <param name="vehicleState"></param> /// <param name="relative"></param> /// <returns></returns> public static PathFollowingBehavior GenerateDefaultTurnBehavior(Lane initial, Lane final, Interconnect turn, bool relative) { CubicBezier[] spline; if (turn.UserPartitions.Count == 2) { Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position; Coordinates p1 = turn.InitialWaypoint.Position; Coordinates p2 = turn.UserPartitions[0].FinalWaypoint.Position; Coordinates p3 = turn.FinalWaypoint.Position; Coordinates p4 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position; Coordinates[] pts = { p0, p1, p2, p3, p4 }; spline = SmoothingSpline.BuildC2Spline(pts, null, null, 0.5); } else if (turn.UserPartitions.Count > 2) { Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position; Coordinates p1 = turn.InitialWaypoint.Position; Coordinates p0head = p0 - p1; p0 = p1 + p0head.Normalize(4); List<Coordinates> middleUsers = new List<Coordinates>(); for (int i = 0; i < turn.UserPartitions.Count - 1; i++) { middleUsers.Add(turn.UserPartitions[i].FinalWaypoint.Position); } Coordinates p2 = turn.FinalWaypoint.Position; Coordinates p3 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position; Coordinates p3head = p3 - p2; p3 = p2 + p3head.Normalize(4); List<Coordinates> finalList = new List<Coordinates>(); finalList.Add(p0); finalList.Add(p1); finalList.AddRange(middleUsers); finalList.Add(p2); finalList.Add(p3); spline = SmoothingSpline.BuildC2Spline(finalList.ToArray(), null, null, 0.5); } else { Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position; Coordinates p1 = turn.InitialWaypoint.Position; Coordinates p3 = turn.FinalWaypoint.Position; Coordinates p4 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position; Coordinates[] pts = { p0, p1, p3, p4 }; spline = SmoothingSpline.BuildC2Spline(pts, null, null, 0.5); } // Create the Path Segments List<IPathSegment> bezierPathSegments = new List<IPathSegment>(); foreach (CubicBezier bezier in spline) { bezierPathSegments.Add(new BezierPathSegment(bezier, null, false)); } // get the method from the road toolkit //IPath turnPath = RoadToolkit.TurnPath(initial, final, turn, vehicleState, relative);// CHANGED IPath turnPath = new Path(bezierPathSegments, CoordinateMode.AbsoluteProjected); // make a speed command (set to 2m/s) SpeedCommand speedCommand = new ScalarSpeedCommand(1); // make behavior //return new PathFollowingBehavior(turnPath, speedCommand); return null; }
public OncomingVehicleDecorator(ScalarSpeedCommand secondarySpeed, double targetDistance, double targetSpeed) { this.secondarySpeed = secondarySpeed; this.targetDistance = targetDistance; this.targetSpeed = targetSpeed; }
/// <summary> /// Makes new parameterization for nav /// </summary> /// <param name="lane"></param> /// <param name="lanePlan"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="stopType"></param> /// <returns></returns> public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance, ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state) { // get min dist double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance; #region Get Decorators // turn direction default ArbiterTurnDirection atd = ArbiterTurnDirection.Straight; List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // check if need decorators if (lane is ArbiterLane && stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) && roadPlan.BestPlan.laneWaypointOfInterest.IsExit && distance < 40.0) { if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null) ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization"); else { switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } else if (lane is SupraLane) { SupraLane sl = (SupraLane)lane; double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position); if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect) { switch (sl.Interconnect.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } #endregion #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new StopAtDistSpeedCommand(distance); #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && stopType != StopType.EndOfLane) { // default behavior Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // exit is next if (stopType == StopType.Exit) { // exit means stopping at a good exit in our current lane IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front); m = new Maneuver(b, nextState, decorators, state.Timestamp); } // stop line is left else if (stopType == StopType.StopLine) { // determine if hte stop line is the best exit bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint); // get turn direction atd = isNavExit ? atd : ArbiterTurnDirection.Straight; // predetermine interconnect if best exit ArbiterInterconnect desired = null; if (isNavExit) desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit; else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25) desired = stopWaypoint.NextPartition.ToInterconnect; // set decorators decorators = isNavExit ? decorators : TurnDecorators.NoDecorators; // stop at the stop IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired); b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); m = new Maneuver(b, nextState, decorators, state.Timestamp); sc = new StopAtLineSpeedCommand(); } else if(stopType == StopType.LastGoal) { // stop at the last goal IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState); m = new Maneuver(b, nextState, decorators, state.Timestamp); } } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // set speed sc = new ScalarSpeedCommand(speed); // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>()); // standard behavior is fine for maneuver m = new Maneuver(b, sisls, decorators, state.Timestamp); } } #endregion #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="parkingSpotPath"></param> /// <param name="spotLeftBound"></param> /// <param name="spotRightBound"></param> /// <param name="spotId"></param> /// <param name="extraDistance"></param> public ZoneParkingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, double extraDistance) : base(zoneId, zonePerimeter, stayOutside, speedCommand) { this.ParkingSpotPath = parkingSpotPath; this.SpotLeftBound = spotLeftBound; this.SpotRightBound = spotRightBound; this.ParkingSpotId = spotId; this.ExtraDistance = extraDistance; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="parkingSpotPath"></param> /// <param name="spotLeftBound"></param> /// <param name="spotRightBound"></param> /// <param name="spotId"></param> /// <param name="extraDistance"></param> /// <param name="endingPolygon"></param> /// <param name="orientations"></param> public ZoneParkingPullOutBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, LinePath reversePath, LinePath finalLeftBound, LinePath finalRightBound) : base(zoneId, zonePerimeter, stayOutside, speedCommand, parkingSpotPath, spotLeftBound, spotRightBound, spotId, 0.0) { this.RecommendedPullOutPath = reversePath; this.EndingLeftBound = finalLeftBound; this.EndingRightBound = finalRightBound; }
/// <summary> /// Parameters to follow the forward vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters Follow(ArbiterLane lane, VehicleState state) { // travelling parameters TravelingParameters tp = new TravelingParameters(); // ignorable obstacles List<int> ignoreVehicle = new List<int>(); ignoreVehicle.Add(CurrentVehicle.VehicleId); // get control parameters ForwardVehicleTrackingControl fvtc = GetControl(lane, state); // init params tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.Decorators = TurnDecorators.NoDecorators; // flag to ignore forward vehicle bool ignoreForward = false; // reversed lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Following Control #region Immediate Stop // need to stop immediately if (fvtc.vFollowing == 0.0) { // don't ignore forward ignoreForward = false; // speed command SpeedCommand sc = new ScalarSpeedCommand(0.0); tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Distance Stop // stop at distance else if (fvtc.vFollowing < 0.7 && CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 && fvtc.xSeparation > fvtc.xAbsMin) { // ignore forward vehicle ignoreForward = true; // speed command SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood); tp.UsingSpeed = false; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Normal Following // else normal else { // ignore the forward vehicle as we are tracking properly ignoreForward = true; // speed command SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing); tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #endregion // check ignore if (ignoreForward) { List<int> ignorable = new List<int>(); ignorable.Add(this.CurrentVehicle.VehicleId); tp.VehiclesToIgnore = ignorable; } else tp.VehiclesToIgnore = new List<int>(); // return parameterization return tp; }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, bool log) { // possible parameterizations List<TravelingParameters> tps = new List<TravelingParameters>(); #region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane // check if the best goal is in the current lane ArbiterWaypoint lanePoint = null; if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane)) lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest; // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint laneNavStop; double laneNavStopSpeed; double laneNavStopDistance; StopType laneNavStopType; this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable, out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop); // create parameterization of the stop TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state); this.navigationParameters = laneNavParams; this.laneParameters = laneNavParams; tps.Add(laneNavParams); #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); } #endregion #endregion #region Forward Vehicle Parameterization // forward vehicle update this.ForwardVehicle.Update(lane, state); // clear current params this.followingParameters = null; // check not in a sparse area bool sparseArea = lane is ArbiterLane ? ((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse : ((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse; // exists forward vehicle if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params, set lane decorators TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable); vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators); this.FollowingParameters = vehicleParams; #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); // set xSeparation CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0"); } #endregion // check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped; bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5; bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1; bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5; if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo) this.ForwardVehicle.StoppedBehindForwardVehicle = true; else { this.ForwardVehicle.StoppedBehindForwardVehicle = false; this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); } // add vehicle param tps.Add(vehicleParams); } else { // no forward vehicle this.followingParameters = null; this.ForwardVehicle.StoppedBehindForwardVehicle = false; } #endregion #region Sparse Waypoint Parameterization // check for sparse waypoints downstream bool sparseDownstream; bool sparseNow; double sparseDistance; lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance); // check if sparse areas downstream if (sparseDownstream) { // set the distance to the sparse area if (sparseNow) sparseDistance = 0.0; // get speed double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front)); // maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new ScalarSpeedCommand(speed); #region Parameterize Given Speed Command // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>()); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = this.laneParameters.Decorators; tp.DistanceToGo = Double.MaxValue; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params tps.Add(tp); #endregion } #endregion // sort params by most urgent tps.Sort(); // set current params this.currentParameters = tps[0]; // get behavior to check add vehicles to ignore if (this.currentParameters.Behavior is StayInLaneBehavior) ((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore; // out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane return tps[0]; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="recommendedPath"></param> /// <param name="endingLeftBound"></param> /// <param name="endingRightBound"></param> public ZoneTravelingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath recommendedPath, LinePath endingLeftBound, LinePath endingRightBound) : base(zoneId, zonePerimeter, stayOutside, speedCommand) { this.RecommendedPath = recommendedPath; this.EndingLeftBound = endingLeftBound; this.EndingRightBound = endingRightBound; }