public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.ignorableObstacles = new List<int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List<int>();
this.decorators = new List<BehaviorDecorator>();
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, Polygon intersectionPolygon, IEnumerable<int> ignorableObstacles, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.intersectionPolygon = intersectionPolygon;
this.ignorableObstacles = ignorableObstacles != null ? new List<int>(ignorableObstacles) : new List<int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List<int>();
this.decorators = new List<BehaviorDecorator>();
}
public StayInLaneBehavior(ArbiterLaneId targetLane, SpeedCommand speedCommand, IEnumerable<int> ignorableObstacles)
{
this.targetLane = targetLane;
this.speedCommand = speedCommand;
if (ignorableObstacles != null)
{
this.ignorableObstacles = new List<int>(ignorableObstacles);
}
else
{
this.ignorableObstacles = new List<int>();
}
}
/// <summary>
/// Check if two zones are equal
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
// make sure type same
if (obj is ArbiterLaneId)
{
// cast
ArbiterLaneId ali = (ArbiterLaneId)obj;
// check if equal
return(ali.Number == this.laneNumber && ali.WayId.Equals(this.WayId));
}
// otherwise not equal
return(false);
}
public StayInLaneBehavior(ArbiterLaneId targetLane, SpeedCommand speedCommand, IEnumerable<int> ignorableObstacles, LinePath backupPath, double laneWidth, int numLanesLeft, int numLanesRight)
{
this.targetLane = targetLane;
this.speedCommand = speedCommand;
if (ignorableObstacles != null) {
this.ignorableObstacles = new List<int>(ignorableObstacles);
}
else {
this.ignorableObstacles = new List<int>();
}
this.backupPath = backupPath;
this.laneWidth = laneWidth;
this.numLanesLeft = numLanesLeft;
this.numLanesRight = numLanesRight;
}
public ChangeLaneBehavior(ArbiterLaneId startLane, ArbiterLaneId targetLane, bool changeLeft, double maxDist, SpeedCommand speedCommand,
IEnumerable<int> ignorableObstacles)
{
this.startLane = startLane;
this.targetLane = targetLane;
this.maxDist = maxDist;
this.speedCommand = speedCommand;
this.changeLeft = changeLeft;
if (ignorableObstacles != null)
{
this.ignorableObstacles = new List<int>(ignorableObstacles);
}
else
{
this.ignorableObstacles = new List<int>();
}
}
/*public SupraLaneBehavior(
ArbiterLaneId startingLaneId, LinePath startingLanePath, double startingLaneWidth, int startingNumLanesLeft, int startingNumLanesRight,
ArbiterLaneId endingLaneId, LinePath endingLanePath, double endingLaneWidth, int endingNumLanesLeft, int endingNumLanesRight,
SpeedCommand speedCommand, List<int> ignorableObstacles) {
this.startingLaneId = startingLaneId;
this.startingLanePath = startingLanePath;
this.startingLaneWidth = startingLaneWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
this.endingLaneId = endingLaneId;
this.endingLanePath = endingLanePath;
this.endingLaneWidth = endingLaneWidth;
this.endingNumLanesLeft = endingNumLanesLeft;
this.endingNumLanesRight = endingNumLanesRight;
this.speedCommand = speedCommand;
this.ignorableObstacles = ignorableObstacles;
}*/
public SupraLaneBehavior(
ArbiterLaneId startingLaneId, LinePath startingLanePath, double startingLaneWidth, int startingNumLanesLeft, int startingNumLanesRight,
ArbiterLaneId endingLaneId, LinePath endingLanePath, double endingLaneWidth, int endingNumLanesLeft, int endingNumLanesRight,
SpeedCommand speedCommand, List<int> ignorableObstacles, Polygon intersectionPolygon)
{
this.startingLaneId = startingLaneId;
this.startingLanePath = startingLanePath;
this.startingLaneWidth = startingLaneWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
this.endingLaneId = endingLaneId;
this.endingLanePath = endingLanePath;
this.endingLaneWidth = endingLaneWidth;
this.endingNumLanesLeft = endingNumLanesLeft;
this.endingNumLanesRight = endingNumLanesRight;
this.intersectionPolygon = intersectionPolygon;
this.speedCommand = speedCommand;
this.ignorableObstacles = ignorableObstacles;
}
public ChangeLaneBehavior(ArbiterLaneId startLane, ArbiterLaneId targetLane, bool changeLeft, double maxDist, SpeedCommand speedCommand,
IEnumerable<int> ignorableObstacles, LinePath backupStartLanePath, LinePath backupTargetLanePath, double startWidth, double targetWidth,
int startingNumLanesLeft, int startingNumLanesRight)
{
this.startLane = startLane;
this.targetLane = targetLane;
this.maxDist = maxDist;
this.speedCommand = speedCommand;
this.changeLeft = changeLeft;
this.backupStartLanePath = backupStartLanePath;
this.backupTargetLanePath = backupTargetLanePath;
this.startWidth = startWidth;
this.targetWidth = targetWidth;
this.startingNumLanesLeft = startingNumLanesLeft;
this.startingNumLanesRight = startingNumLanesRight;
if (ignorableObstacles != null) {
this.ignorableObstacles = new List<int>(ignorableObstacles);
}
else {
this.ignorableObstacles = new List<int>();
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneId"></param>
public ArbiterLane(ArbiterLaneId laneId)
{
this.LaneId = laneId;
this.Waypoints = new Dictionary<ArbiterWaypointId, ArbiterWaypoint>();
this.SafetyZones = new List<ArbiterSafetyZone>();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="target"></param>
public InternalState(ArbiterLaneId initial, ArbiterLaneId target, Probability confidence)
{
this.Initial = initial;
this.Target = target;
this.Confidence = confidence;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="target"></param>
public InternalState(ArbiterLaneId initial, ArbiterLaneId target)
{
this.Initial = initial;
this.Target = target;
this.Confidence = new Probability(0.8, 0.2);
}
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;
}
}
/// <summary>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if (sl.LaneWidth == 0)
{
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
}
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count - 1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List <IPathSegment> ips = new List <IPathSegment>();
List <Coordinates> pathSegments = new List <Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach (ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if (aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return(arn);
}
public int NumberOfLanesLeft(Coordinates position, bool forwards)
{
if (forwards)
{
if (this.LaneOnLeft != null)
{
// our lane #
int n = this.LaneId.Number;
// left lane #
int l = this.LaneOnLeft.LaneId.Number;
// count
int count = 0;
// if left lane numbers going down
if (n > l)
{
// loop over left lanes going down
for (int i = n - 1; i > 0; i--)
{
// determine who is close to this position
ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);
// get lane
ArbiterLane lane = null;
if (this.Way.Segment.Lanes.ContainsKey(lane1))
{
lane = this.Way.Segment.Lanes[lane1];
}
else if (this.Way.Segment.Lanes.ContainsKey(lane2))
{
lane = this.Way.Segment.Lanes[lane2];
}
// check lane exists
if (lane != null)
{
// determine if close and inside
bool close = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
bool inside = lane.IsInside(position);
// add if both
if (close && inside)
{
count++;
}
}
}
}
else
{
// loop over left lanes
for (int i = n + 1; i <= this.Way.Segment.Lanes.Count; i++)
{
// determine who is close to this position
ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);
// get lane
ArbiterLane lane = null;
if (this.Way.Segment.Lanes.ContainsKey(lane1))
{
lane = this.Way.Segment.Lanes[lane1];
}
else if (this.Way.Segment.Lanes.ContainsKey(lane2))
{
lane = this.Way.Segment.Lanes[lane2];
}
// check lane exists
if (lane != null)
{
// determine if close and inside
bool close = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
bool inside = lane.IsInside(position);
// add if both
if (close && inside)
{
count++;
}
}
}
}
// return the count
return(count);
}
else
{
return(0);
}
}
else
{
return(this.NumberOfLanesRight(position, true));
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypointNumber"></param>
/// <param name="laneId"></param>
public ArbiterWaypointId(int waypointNumber, ArbiterLaneId laneId)
{
this.waypointNumber = waypointNumber;
this.LaneId = laneId;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypointNumber"></param>
/// <param name="laneId"></param>
public ArbiterWaypointId(int waypointNumber, ArbiterLaneId laneId)
{
this.waypointNumber = waypointNumber;
this.LaneId = laneId;
}
/// <summary>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if(sl.LaneWidth == 0)
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List<ArbiterWaypoint> waypointList = new List<ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List<ArbiterLanePartition> alps = new List<ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count-1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i+1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List<ArbiterUserPartition> aups = new List<ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List<IPathSegment> ips = new List<IPathSegment>();
List<Coordinates> pathSegments = new List<Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach(ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if(aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return arn;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="laneId"></param>
public ArbiterLane(ArbiterLaneId laneId)
{
this.LaneId = laneId;
this.Waypoints = new Dictionary <ArbiterWaypointId, ArbiterWaypoint>();
this.SafetyZones = new List <ArbiterSafetyZone>();
}
public int NumberOfLanesRight(Coordinates position, bool forwards)
{
if (forwards)
{
if (this.LaneOnRight != null)
{
// our lane #
int n = this.LaneId.Number;
// left lane #
int l = this.LaneOnRight.LaneId.Number;
// count
int count = 0;
// if left lane numbers going down
if (n > l)
{
// loop over left lanes going down
for (int i = n - 1; i >= 0; i--)
{
// determine who is close to this position
ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);
// get lane
ArbiterLane lane = null;
if (this.Way.Segment.Lanes.ContainsKey(lane1))
lane = this.Way.Segment.Lanes[lane1];
else if (this.Way.Segment.Lanes.ContainsKey(lane2))
lane = this.Way.Segment.Lanes[lane2];
// check lane exists
if (lane != null)
{
// determine if close and inside
bool close = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
bool inside = lane.IsInside(position);
// add if both
if (close && inside)
count++;
}
}
}
else
{
// loop over left lanes
for (int i = n + 1; i < this.Way.Segment.Lanes.Count; i++)
{
// determine who is close to this position
ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);
// get lane
ArbiterLane lane = null;
if (this.Way.Segment.Lanes.ContainsKey(lane1))
lane = this.Way.Segment.Lanes[lane1];
else if (this.Way.Segment.Lanes.ContainsKey(lane2))
lane = this.Way.Segment.Lanes[lane2];
// check lane exists
if (lane != null)
{
// determine if close and inside
bool close = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
bool inside = lane.IsInside(position);
// add if both
if (close && inside)
count++;
}
}
}
// return the count
return count;
}
else
{
return 0;
}
}
else
{
return this.NumberOfLanesLeft(position, true);
}
}
private void HandleBehavior(SupraLaneBehavior cb)
{
// get the transform to make the paths vehicle-relative
AbsoluteTransformer transform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
this.behaviorTimestamp = transform.Timestamp;
this.startingLaneID = cb.StartingLaneId;
this.startingLanePath = cb.StartingLanePath.Transform(transform);
this.startingLaneWidth = cb.StartingLaneWidth;
this.startingLanesLeft = cb.StartingNumLanesLeft;
this.startingLanesRight = cb.StartingNumLanesRight;
this.endingLaneID = cb.EndingLaneId;
this.endingLanePath = cb.EndingLanePath.Transform(transform);
this.endingLaneWidth = cb.EndingLaneWidth;
this.endingLanesLeft = cb.EndingNumLanesLeft;
this.endingLanesRight = cb.EndingNumLanesRight;
this.ignorableObstacles = cb.IgnorableObstacles;
Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles;
if (cb.IntersectionPolygon != null) {
this.intersectionPolygon = cb.IntersectionPolygon.Transform(transform);
// set the polygon to the ui
Services.UIService.PushPolygon(cb.IntersectionPolygon, cb.TimeStamp, "intersection polygon", false);
}
else {
this.intersectionPolygon = null;
}
HandleSpeedCommand(cb.SpeedCommand);
opposingLaneVehicleExists = false;
oncomingVehicleExists = false;
extraWidth = 0;
if (cb.Decorators != null) {
foreach (BehaviorDecorator d in cb.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 || startingLanesRight > 0) {
oncomingVehicleExists = false;
}
else {
HandleSpeedCommand(oncomingVehicleSpeedCommand);
}
}
if (startingLaneID != null && Services.RoadNetwork != null) {
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[startingLaneID.SegmentId].Lanes[startingLaneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
sparse = (lane.GetClosestPartition(pose.xy).Type == PartitionType.Sparse);
if (sparse) {
LinePath.PointOnPath closest = cb.StartingLanePath.GetClosestPoint(pose.xy);
goalPoint = cb.StartingLanePath[closest.Index+1];
}
}
if (sparse) {
if (Services.ObstaclePipeline.ExtraSpacing == 0) {
Services.ObstaclePipeline.ExtraSpacing = 0.5;
}
Services.ObstaclePipeline.UseOccupancyGrid = false;
}
else {
Services.ObstaclePipeline.ExtraSpacing = 0;
smootherSpacingAdjust = 0;
Services.ObstaclePipeline.UseOccupancyGrid = true;
}
}
private void HandleBehavior(UTurnBehavior cb)
{
// get the absolute transform
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
this.polygonTimestamp = absTransform.Timestamp;
this.polygon = cb.Boundary.Transform(absTransform);
this.finalLane = cb.EndingLane;
this.finalSpeedCommand = cb.EndingSpeedCommand;
this.stopOnLine = cb.StopOnEndingPath;
this.stayOutPolygons = cb.StayOutPolygons;
// run constant checking if we're supposed to stop on the final line
if (stopOnLine) {
checkMode = true;
}
else {
checkMode = false;
}
// transform the path
LinePath.PointOnPath closestPoint = cb.EndingPath.GetClosestPoint(originalPoint);
// relativize the path
LinePath relFinalPath = cb.EndingPath.Transform(absTransform);
// get the ending orientation
finalOrientation = new LineSegment(relFinalPath[closestPoint.Index], relFinalPath[closestPoint.Index+1]);
Services.UIService.PushLineList(cb.EndingPath, cb.TimeStamp, "original path1", false);
}
private void HandleBehavior(ChangeLaneBehavior cb)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp);
startingLanePath = cb.BackupStartLanePath.Transform(absTransform);
endingLanePath = cb.BackupTargetLanePath.Transform(absTransform);
startingNumLanesLeft = cb.StartingNumLanesLeft;
startingNumLanesRight = cb.StartingNumLaneRights;
startingLaneWidth = cb.StartLaneWidth;
endingLaneWidth = cb.TargetLaneWidth;
startingLaneID = cb.StartLane;
endingLaneID = cb.TargetLane;
changeLeft = cb.ChangeLeft;
if (cb.MaxDist > -TahoeParams.FL) {
dist = cb.MaxDist + TahoeParams.FL;
}
else {
dist = 0;
}
speedCommand = cb.SpeedCommand;
ignorableObstacles = cb.IgnorableObstacles;
Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles;
behaviorTimestamp = absTransform.Timestamp;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialId"></param>
/// <param name="finalId"></param>
public ArbiterLanePartitionId(ArbiterWaypointId initialId, ArbiterWaypointId finalId, ArbiterLaneId laneId)
{
this.InitialId = initialId;
this.FinalId = finalId;
this.LaneId = laneId;
}
/// <summary>
/// Generates lane adjacency in the network
/// </summary>
/// <param name="arn"></param>
private void GenerateLaneAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// check if both ways exist for first algorithm
if (asg.Way1.IsValid && asg.Way2.IsValid)
{
// lanes of the segment
Dictionary<ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// get a sample lane from way 1
Dictionary<ArbiterLaneId, ArbiterLane>.Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator();
way1Enum.MoveNext();
ArbiterLane way1Sample = way1Enum.Current.Value;
// get a sample lane from way 2
Dictionary<ArbiterLaneId, ArbiterLane>.Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator();
way2Enum.MoveNext();
ArbiterLane way2Sample = way2Enum.Current.Value;
// direction, 1 means way1 has lower # lanes
int modifier = 1;
// check if way 2 has lower lane numbers
if (way1Sample.LaneId.Number > way2Sample.LaneId.Number)
{
// set modifier to -1 so count other way
modifier = -1;
}
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// if not lane 1
if (al.LaneId.Number != 1)
{
// get lower # lane in way 1
ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId);
// check if the segment contains this lane
if (segLanes.ContainsKey(lowerNumWay1Id))
{
// get lane
ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id];
// check if current lane is also in way 1
if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to right
if (modifier == 1)
{
al.LaneOnRight = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
// otherwise -1 => lane is to left
else
{
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnRight = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
}
// otherwise the lowe lane is in way 2
else
{
// set lane
ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)];
// check if current lane is also in way 2
if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to left
if (modifier == 1)
{
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnRight = al;
}
// otherwise -1 => lane is to right
else
{
al.LaneOnRight = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
}
} // loop over lanes
} // both ways valid
// otherwise only a single way is valid
else
{
// lanes of the segment
Dictionary<ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// make sure more than one lane exists
if(segLanes.Count > 1)
{
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// get theoretical id of lane one number up
ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId);
// check if lane one number up exists
if(segLanes.ContainsKey(ali))
{
// get lane one number up
ArbiterLane alu = segLanes[ali];
// check # waypoints
if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1)
{
// get closest points on this lane and other lane
PointOnPath p1;
PointOnPath p2;
double distance;
CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance);
// get partition points of closest point on this lane
Coordinates partitionStart = p1.segment.Start;
Coordinates partitionEnd = p1.segment.End;
// get area of partition triangle
double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd);
// determine if closest point on other lane is to the left or right of partition
bool onLeft = true;
if (triangeArea >= 0)
{
onLeft = false;
}
// set adjacency accordingly for both lanes
if (onLeft)
{
al.LaneOnLeft = alu;
alu.LaneOnRight = al;
}
// otherwise on right
else
{
al.LaneOnRight = alu;
alu.LaneOnLeft = al;
}
}
}
}
}
} // end single way only valid
// loop over lanes to print info on adjacency
/*foreach (ArbiterLane al in asg.Lanes.Values)
{
Console.Write(al.LaneId.ToString() + ": ");
if (al.LaneOnLeft != null)
{
Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString());
}
if (al.LaneOnRight != null)
{
Console.Write("Right-" + al.LaneOnRight.LaneId.ToString());
}
Console.Write("\n");
}
*/
} // segment loop
}
/// <summary>
/// Generates lane adjacency in the network
/// </summary>
/// <param name="arn"></param>
private void GenerateLaneAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// check if both ways exist for first algorithm
if (asg.Way1.IsValid && asg.Way2.IsValid)
{
// lanes of the segment
Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// get a sample lane from way 1
Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator();
way1Enum.MoveNext();
ArbiterLane way1Sample = way1Enum.Current.Value;
// get a sample lane from way 2
Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator();
way2Enum.MoveNext();
ArbiterLane way2Sample = way2Enum.Current.Value;
// direction, 1 means way1 has lower # lanes
int modifier = 1;
// check if way 2 has lower lane numbers
if (way1Sample.LaneId.Number > way2Sample.LaneId.Number)
{
// set modifier to -1 so count other way
modifier = -1;
}
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// if not lane 1
if (al.LaneId.Number != 1)
{
// get lower # lane in way 1
ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId);
// check if the segment contains this lane
if (segLanes.ContainsKey(lowerNumWay1Id))
{
// get lane
ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id];
// check if current lane is also in way 1
if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to right
if (modifier == 1)
{
al.LaneOnRight = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
// otherwise -1 => lane is to left
else
{
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnRight = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
}
// otherwise the lowe lane is in way 2
else
{
// set lane
ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)];
// check if current lane is also in way 2
if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to left
if (modifier == 1)
{
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnRight = al;
}
// otherwise -1 => lane is to right
else
{
al.LaneOnRight = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
}
} // loop over lanes
} // both ways valid
// otherwise only a single way is valid
else
{
// lanes of the segment
Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// make sure more than one lane exists
if (segLanes.Count > 1)
{
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// get theoretical id of lane one number up
ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId);
// check if lane one number up exists
if (segLanes.ContainsKey(ali))
{
// get lane one number up
ArbiterLane alu = segLanes[ali];
// check # waypoints
if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1)
{
// get closest points on this lane and other lane
PointOnPath p1;
PointOnPath p2;
double distance;
CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance);
// get partition points of closest point on this lane
Coordinates partitionStart = p1.segment.Start;
Coordinates partitionEnd = p1.segment.End;
// get area of partition triangle
double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd);
// determine if closest point on other lane is to the left or right of partition
bool onLeft = true;
if (triangeArea >= 0)
{
onLeft = false;
}
// set adjacency accordingly for both lanes
if (onLeft)
{
al.LaneOnLeft = alu;
alu.LaneOnRight = al;
}
// otherwise on right
else
{
al.LaneOnRight = alu;
alu.LaneOnLeft = al;
}
}
}
}
}
} // end single way only valid
// loop over lanes to print info on adjacency
/*foreach (ArbiterLane al in asg.Lanes.Values)
* {
* Console.Write(al.LaneId.ToString() + ": ");
*
* if (al.LaneOnLeft != null)
* {
* Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString());
* }
*
* if (al.LaneOnRight != null)
* {
* Console.Write("Right-" + al.LaneOnRight.LaneId.ToString());
* }
*
* Console.Write("\n");
* }
*/
} // segment loop
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialId"></param>
/// <param name="finalId"></param>
public ArbiterLanePartitionId(ArbiterWaypointId initialId, ArbiterWaypointId finalId, ArbiterLaneId laneId)
{
this.InitialId = initialId;
this.FinalId = finalId;
this.LaneId = laneId;
}
