/// <summary>
/// Constructor
/// </summary>
/// <param name="tacticalBlockage"></param>
/// <param name="currentState"></param>
public EncounteredBlockageState(ITacticalBlockage tacticalBlockage, IState planningState, BlockageRecoveryDEFCON defcon, SAUDILevel saudi)
{
this.TacticalBlockage = tacticalBlockage;
this.PlanningState = planningState;
this.Saudi = saudi;
this.Defcon = defcon;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="tacticalBlockage"></param>
/// <param name="currentState"></param>
public EncounteredBlockageState(ITacticalBlockage tacticalBlockage, IState planningState)
{
this.TacticalBlockage = tacticalBlockage;
this.PlanningState = planningState;
this.Saudi = SAUDILevel.None;
this.Defcon = BlockageRecoveryDEFCON.INITIAL;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="tacticalBlockage"></param>
/// <param name="currentState"></param>
public EncounteredBlockageState(ITacticalBlockage tacticalBlockage, IState planningState)
{
this.TacticalBlockage = tacticalBlockage;
this.PlanningState = planningState;
this.Saudi = SAUDILevel.None;
this.Defcon = BlockageRecoveryDEFCON.INITIAL;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="tacticalBlockage"></param>
/// <param name="currentState"></param>
public EncounteredBlockageState(ITacticalBlockage tacticalBlockage, IState planningState, BlockageRecoveryDEFCON defcon, SAUDILevel saudi)
{
this.TacticalBlockage = tacticalBlockage;
this.PlanningState = planningState;
this.Saudi = saudi;
this.Defcon = defcon;
}
public TrajectoryBlockedReport(CompletionResult result, double distToBlockage, BlockageType blockageType, int trackID, bool reverseRecommended, Type behaviorType, string behaviorId)
: base(behaviorType, behaviorId)
{
this.result = result;
this.distToBlockage = distToBlockage;
this.blockageType = blockageType;
this.trackID = trackID;
this.reverseRecommended = reverseRecommended;
this.saudiLevel = SAUDILevel.None;
}
public TrajectoryBlockedReport(CompletionResult result, double distToBlockage, BlockageType blockageType, int trackID, bool reverseRecommended, Type behaviorType, string behaviorId)
: base(behaviorType, behaviorId)
{
this.result = result;
this.distToBlockage = distToBlockage;
this.blockageType = blockageType;
this.trackID = trackID;
this.reverseRecommended = reverseRecommended;
this.saudiLevel = SAUDILevel.None;
}
public TrajectoryBlockedReport(Type behaviorType)
: base(behaviorType)
{
this.badStuffFlag = true;
this.trackID = -1;
this.blockageType = BlockageType.Static;
this.reverseRecommended = true;
this.distToBlockage = double.NaN;
this.saudiLevel = SAUDILevel.L1;
this.result = CompletionResult.Stopped;
}
public TrajectoryBlockedReport(Type behaviorType)
: base(behaviorType)
{
this.badStuffFlag = true;
this.trackID = -1;
this.blockageType = BlockageType.Static;
this.reverseRecommended = true;
this.distToBlockage = double.NaN;
this.saudiLevel = SAUDILevel.L1;
this.result = CompletionResult.Stopped;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
private void HandleDecorators(List <BehaviorDecorator> decorators)
{
if (decorators == null)
{
return;
}
saudiLevel = SAUDILevel.None;
foreach (BehaviorDecorator dec in decorators)
{
if (dec is TurnSignalDecorator)
{
HandleTurnSignal((TurnSignalDecorator)dec);
}
else if (dec is ShutUpAndDoItDecorator)
{
saudiLevel = ((ShutUpAndDoItDecorator)dec).Level;
}
else
{
//TraceSource.TraceEvent(TraceEventType.Warning, 7, "unknown decorator: {0}", dec);
}
}
}
public ObstacleCollection GetProcessedObstacles(CarTimestamp timestamp, SAUDILevel saudi)
{
List<Obstacle> transformedObstacles = new List<Obstacle>();
if (Services.StateProvider.GetVehicleState().speed < 4.5) {
Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles);
Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles);
if (leftObstacle != null) {
transformedObstacles.Add(leftObstacle);
}
if (rightObstacle != null) {
transformedObstacles.Add(rightObstacle);
}
}
ObstacleCollection processedObstacles = this.processedObstacles;
if (processedObstacles != null) {
if (transformedObstacles.Capacity < transformedObstacles.Count + processedObstacles.obstacles.Count) {
transformedObstacles.Capacity = transformedObstacles.Count + processedObstacles.obstacles.Count;
}
RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, timestamp);
foreach (Obstacle obs in processedObstacles.obstacles) {
if (!Settings.IgnoreTracks || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.StaticSmall) {
Obstacle newObs = obs.ShallowClone();
if (saudi == SAUDILevel.L1) {
if (obs.obstacleClass == ObstacleClass.StaticSmall && Math.Abs(newObs.AvoidancePolygon.GetArea()) < L1_area_threshold)
continue;
}
else if (saudi == SAUDILevel.L2) {
if (obs.obstacleClass == ObstacleClass.StaticSmall)
continue;
}
else if (saudi == SAUDILevel.L3) {
if (obs.obstacleClass == ObstacleClass.StaticSmall || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.DynamicNotCarlike || obs.obstacleClass == ObstacleClass.DynamicUnknown)
continue;
}
if (newObs.cspacePolygon != null) newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform);
if (newObs.extrudedPolygon != null) newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform);
if (newObs.obstaclePolygon != null) newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform);
if (newObs.predictedPolygon != null) newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform);
transformedObstacles.Add(newObs);
}
}
}
return new ObstacleCollection(timestamp, transformedObstacles);
}
/// <summary>
/// Reverse because of a blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <param name="laneOpposing"></param>
/// <param name="currentBlockage"></param>
/// <param name="ebs"></param>
/// <returns></returns>
private Maneuver LaneReverseManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
BlockageRecoveryDEFCON defcon, SAUDILevel saudi, bool laneOpposing, ITacticalBlockage currentBlockage, EncounteredBlockageState ebs)
{
// distance to reverse
double distToReverse = TahoeParams.VL * 2.0;
// just reverse and let brian catch it
StayInLaneBehavior reverseBehavior = new StayInLaneBehavior(
lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true),
new List<int>(), lane.LanePath(), lane.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// get the saudi level
List<BehaviorDecorator> decs = new List<BehaviorDecorator>(TurnDecorators.HazardDecorator.ToArray());
decs.Add(new ShutUpAndDoItDecorator(saudi));
reverseBehavior.Decorators = decs;
// state
IState laneState = new StayInLaneState(lane, CoreCommon.CorePlanningState);
BlockageRecoveryState brs = new BlockageRecoveryState(
reverseBehavior, laneState, laneState, defcon, ebs, BlockageRecoverySTATUS.EXECUTING);
// check enough room in lane to reverse
RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;
if (rearMonitor == null || !rearMonitor.lane.Equals(lane))
this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor = new RearQuadrantMonitor(lane, SideObstacleSide.Driver);
#region Start too close to start of lane
// check distance to the start of the lane
double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front);
if (laneStartDistanceFromFront < TahoeParams.VL)
{
// make sure we're not at the very start of the lane
if (laneStartDistanceFromFront < 0.5)
{
// output
ArbiterOutput.Output("Too close to the start of the lane, raising defcon");
// go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
// otherwise back up to the start of the lane
else
{
// output
ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL");
// set proper distance
distToReverse = TahoeParams.VL;
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
}
#endregion
#region Sparse
// check distance to the start of the lane
if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse)
{
// set behavior speed (no car behind us as too close to start of lane)
LinePath bp = vehicleState.VehicleLinePath;
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
}
#endregion
#region Vehicle Behind us
// update and check for vehicle
rearMonitor.Update(vehicleState);
if (rearMonitor.CurrentVehicle != null)
{
// check for not failed forward vehicle
if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed)
{
// check if enough room to rear vehicle
double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear);
if (vehicleDistance < distToReverse - 2.0)
{
// revised distance given vehicle
double revisedDistance = vehicleDistance - 2.0;
// check enough room
if (revisedDistance > TahoeParams.VL)
{
// set the updated speed command
reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true);
}
// not enough room
else
{
// output not enough room because of vehicle
ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear");
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// check failed rear vehicle, up defcon
else
{
// failed vehicle in rear, go up chain
return new Maneuver(new NullBehavior(),
new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
TurnDecorators.NoDecorators,
vehicleState.Timestamp);
}
}
#endregion
// all clear, return reverse maneuver, set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
return new Maneuver(reverseBehavior, brs, decs, vehicleState.Timestamp);
}
public ShutUpAndDoItDecorator(SAUDILevel level)
{
this.level = level;
}
public ShutUpAndDoItDecorator()
{
level = SAUDILevel.L1;
}
private void HandleDecorators(List<BehaviorDecorator> decorators)
{
if (decorators == null)
return;
saudiLevel = SAUDILevel.None;
foreach (BehaviorDecorator dec in decorators) {
if (dec is TurnSignalDecorator) {
HandleTurnSignal((TurnSignalDecorator)dec);
}
else if (dec is ShutUpAndDoItDecorator) {
saudiLevel = ((ShutUpAndDoItDecorator)dec).Level;
}
else {
//TraceSource.TraceEvent(TraceEventType.Warning, 7, "unknown decorator: {0}", dec);
}
}
}
public ShutUpAndDoItDecorator(SAUDILevel level)
{
this.level = level;
}
public ShutUpAndDoItDecorator()
{
level = SAUDILevel.L1;
}
public ObstacleCollection GetProcessedObstacles(CarTimestamp timestamp, SAUDILevel saudi)
{
List <Obstacle> transformedObstacles = new List <Obstacle>();
if (Services.StateProvider.GetVehicleState().speed < 4.5)
{
Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles);
Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles);
if (leftObstacle != null)
{
transformedObstacles.Add(leftObstacle);
}
if (rightObstacle != null)
{
transformedObstacles.Add(rightObstacle);
}
}
ObstacleCollection processedObstacles = this.processedObstacles;
if (processedObstacles != null)
{
if (transformedObstacles.Capacity < transformedObstacles.Count + processedObstacles.obstacles.Count)
{
transformedObstacles.Capacity = transformedObstacles.Count + processedObstacles.obstacles.Count;
}
RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, timestamp);
foreach (Obstacle obs in processedObstacles.obstacles)
{
if (!Settings.IgnoreTracks || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.StaticSmall)
{
Obstacle newObs = obs.ShallowClone();
if (saudi == SAUDILevel.L1)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall && Math.Abs(newObs.AvoidancePolygon.GetArea()) < L1_area_threshold)
{
continue;
}
}
else if (saudi == SAUDILevel.L2)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall)
{
continue;
}
}
else if (saudi == SAUDILevel.L3)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.DynamicNotCarlike || obs.obstacleClass == ObstacleClass.DynamicUnknown)
{
continue;
}
}
if (newObs.cspacePolygon != null)
{
newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform);
}
if (newObs.extrudedPolygon != null)
{
newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform);
}
if (newObs.obstaclePolygon != null)
{
newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform);
}
if (newObs.predictedPolygon != null)
{
newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform);
}
transformedObstacles.Add(newObs);
}
}
}
return(new ObstacleCollection(timestamp, transformedObstacles));
}