public PathFollowingBehavior(Path basePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand)
{
this.basePath = basePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
}
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);
}
public PathFollowingBehavior(Path basePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand)
{
this.basePath = basePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
}
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>();
}
}
public UTurnBehavior(Polygon boundary, LinePath endingPath, ArbiterLaneId endingLane, SpeedCommand endingSpeedCommand)
{
this.boundary = boundary;
this.endingPath = endingPath;
this.endingLane = endingLane;
this.endingSpeedCommand = endingSpeedCommand;
this.stayOutPolygons = new List <Polygon>();
}
public UTurnBehavior(Polygon boundary, LinePath endingPath, ArbiterLaneId endingLane, SpeedCommand endingSpeedCommand, List <Polygon> stayOutPolygons)
{
this.boundary = boundary;
this.endingPath = endingPath;
this.endingLane = endingLane;
this.endingSpeedCommand = endingSpeedCommand;
this.stayOutPolygons = stayOutPolygons;
this.stopOnEndingPath = true;
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
LinePath leftBound, LinePath rightBound,
double maxSpeed, double?endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double?approachSpeed)
{
SmoothAndTrack(basePath, useTargetPath, new LinePath[] { leftBound }, new LinePath[] { rightBound }, maxSpeed, endingHeading, endingOffsetBound, curTimestamp, doAvoidance, speedCommand, behaviorTimestamp, commandLabel, ref cancelled, ref previousSmoothedPath, ref previousSmoothedPathTimestamp, ref approachSpeed);
}
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>();
}
/// <summary>
/// Constructs the path following with the specified path and with the specified speed command.
/// </summary>
/// <param name="path">Path to follow.</param>
/// <param name="coordMode">Form of the coordinates in path.</param>
/// <param name="speedCommand">Speed command associated with the command.</param>
public PathFollowingBehavior(IPath path, SpeedCommand speedCommand)
{
if (speedCommand == null)
throw new ArgumentNullException("speedCommand");
if (path == null)
throw new ArgumentNullException("path");
if (path.Count == 0)
throw new ArgumentException("Path must have at least one segment", "path");
this.path = path;
this.speedCommand = speedCommand;
}
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>();
}
}
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>();
}
}
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>();
}
/// <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;
}
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;
}
/// <summary>
/// Gets the behavior
/// </summary>
/// <param name="sc"></param>
/// <param name="fron"></param>
/// <returns></returns>
public SupraLaneBehavior GetBehavior(SpeedCommand sc, Coordinates front, List <int> ignorable)
{
LinePath initPath = this.Lane.InitialPath(front);
LinePath finPath = this.Lane.FinalPath(front);
int il = Lane.Initial.NumberOfLanesLeft(front, true);
int ir = Lane.Initial.NumberOfLanesRight(front, true);
int fl = Lane.Final.NumberOfLanesLeft(front, true);
int fr = Lane.Final.NumberOfLanesRight(front, true);
SupraLaneBehavior slb = new SupraLaneBehavior(
Lane.Initial.LaneId, initPath, Lane.Initial.Width, il, ir,
Lane.Final.LaneId, finPath, Lane.Final.Width, fl, fr, sc, ignorable, this.Lane.IntersectionPolygon);
return(slb);
}
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;
}
/// <summary>
/// Constructs the path following with the specified path and with the specified speed command.
/// </summary>
/// <param name="path">Path to follow.</param>
/// <param name="coordMode">Form of the coordinates in path.</param>
/// <param name="speedCommand">Speed command associated with the command.</param>
public PathFollowingBehavior(IPath path, SpeedCommand speedCommand)
{
if (speedCommand == null)
{
throw new ArgumentNullException("speedCommand");
}
if (path == null)
{
throw new ArgumentNullException("path");
}
if (path.Count == 0)
{
throw new ArgumentException("Path must have at least one segment", "path");
}
this.path = path;
this.speedCommand = speedCommand;
}
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 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 static double GetPlanningDistance(CarTimestamp curTimestamp, SpeedCommand speedCommand, CarTimestamp speedCommandTimestamp)
{
if (speedCommand is ScalarSpeedCommand)
{
// figure out the commanded speed and plan for the stopping distance
double commandedSpeed = ((ScalarSpeedCommand)speedCommand).Speed;
// calculate expected stopping distance (actual deceleration at 2 m/s^2 + system latency + tahoe rear-axle to front-bumper length)
double planningDist = commandedSpeed * commandedSpeed / (2 * planning_dist_decel) + commandedSpeed * planning_dist_sys_latency + TahoeParams.FL;
//if (planningDist < TahoeParams.FL + TahoeParams.VL) {
// planningDist = TahoeParams.FL + TahoeParams.VL;
//}
if (planningDist < planning_dist_min)
{
planningDist = planning_dist_min;
}
else if (planningDist > planning_dist_max)
{
planningDist = planning_dist_max;
}
return(planningDist);
}
else if (speedCommand is StopAtDistSpeedCommand)
{
// transform the distance to the current timestamp
double origDist = ((StopAtDistSpeedCommand)speedCommand).Distance;
double remainingDist = origDist - Services.TrackedDistance.GetDistanceTravelled(speedCommandTimestamp, curTimestamp);
if (remainingDist < 0)
{
remainingDist = 0;
}
return(remainingDist + TahoeParams.FL);
}
else if (speedCommand is StopAtLineSpeedCommand)
{
double remainingDist = Services.Stopline.DistanceToStopline();
return(remainingDist);
}
else
{
throw new InvalidOperationException();
}
}
/*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 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>();
}
}
public static double GetPlanningDistance(CarTimestamp curTimestamp, SpeedCommand speedCommand, CarTimestamp speedCommandTimestamp)
{
if (speedCommand is ScalarSpeedCommand) {
// figure out the commanded speed and plan for the stopping distance
double commandedSpeed = ((ScalarSpeedCommand)speedCommand).Speed;
// calculate expected stopping distance (actual deceleration at 2 m/s^2 + system latency + tahoe rear-axle to front-bumper length)
double planningDist = commandedSpeed*commandedSpeed/(2*planning_dist_decel) + commandedSpeed*planning_dist_sys_latency + TahoeParams.FL;
//if (planningDist < TahoeParams.FL + TahoeParams.VL) {
// planningDist = TahoeParams.FL + TahoeParams.VL;
//}
if (planningDist < planning_dist_min) {
planningDist = planning_dist_min;
}
else if (planningDist > planning_dist_max) {
planningDist = planning_dist_max;
}
return planningDist;
}
else if (speedCommand is StopAtDistSpeedCommand) {
// transform the distance to the current timestamp
double origDist = ((StopAtDistSpeedCommand)speedCommand).Distance;
double remainingDist = origDist - Services.TrackedDistance.GetDistanceTravelled(speedCommandTimestamp, curTimestamp);
if (remainingDist < 0) {
remainingDist = 0;
}
return remainingDist + TahoeParams.FL;
}
else if (speedCommand is StopAtLineSpeedCommand) {
double remainingDist = Services.Stopline.DistanceToStopline();
return remainingDist;
}
else {
throw new InvalidOperationException();
}
}
protected void HandleSpeedCommand(SpeedCommand cmd)
{
string speedCommandString = null;
this.speedCommand = cmd;
reverseGear = false;
if (cmd is ScalarSpeedCommand) {
Services.Dataset.ItemAs<double>("scalar speed").Add(((ScalarSpeedCommand)cmd).Speed, curTimestamp);
speedCommandString = string.Format("Scalar Speed ({0:F1} m/s)", ((ScalarSpeedCommand)speedCommand).Speed);
}
else if (cmd is StopAtDistSpeedCommand) {
StopAtDistSpeedCommand stopCmd = (StopAtDistSpeedCommand)cmd;
if (stopCmd.Reverse) {
reverseGear = true;
}
else {
reverseGear = false;
}
speedCommandString = string.Format("Stop At Dist ({0}{1:F1} m)", reverseGear ? "rev " : "", stopCmd.Distance);
}
else if (cmd is StopAtLineSpeedCommand) {
speedCommandString = string.Format("Stop At Line ({0:F1} m)", Services.Stopline.DistanceToStopline());
}
if (speedCommandString != null) {
Services.Dataset.ItemAs<string>("speed command").Add(speedCommandString, curTimestamp);
}
Services.Dataset.ItemAs<double>("commanded stop distance").Add(GetSpeedCommandStopDistance(), curTimestamp);
}
/// <summary>
/// Gets primary maneuver given our position and the turn we are traveling upon
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState)
{
#region Check are planning over the correct turn
if (CoreCommon.CorePlanningState is TurnState)
{
TurnState ts = (TurnState)CoreCommon.CorePlanningState;
if (this.turn == null || !this.turn.Equals(ts.Interconnect))
{
this.turn = ts.Interconnect;
this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null);
}
else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect))
{
this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null);
}
}
#endregion
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not at highest level already
if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds)
{
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic ||
(TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) &&
TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped))
{
// go to a blockage handling tactical
return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring");
}
}
else
{
ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report");
}
}
#endregion
#region Intersection Check
if (!this.CanGo(vehicleState))
{
if (turn.FinalGeneric is ArbiterWaypoint)
{
TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false);
return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL);
// hold brake
IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0));
TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId);
return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Final is Lane Waypoint
if (turn.FinalGeneric is ArbiterWaypoint)
{
// final point
ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric;
// plan down entry lane
RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>());
// point of interest downstream
DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;
// get path this represents
List <Coordinates> pathCoordinates = new List <Coordinates>();
pathCoordinates.Add(vehicleState.Position);
foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1]))
{
pathCoordinates.Add(aw.Position);
}
LinePath lp = new LinePath(pathCoordinates);
// list of all parameterizations
List <TravelingParameters> parameterizations = new List <TravelingParameters>();
// get lane navigation parameterization
TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp);
parameterizations.Add(navigationParameters);
// update forward tracker and get vehicle parameterizations if forward vehicle exists
this.forwardMonitor.Update(vehicleState, final, lp);
if (this.forwardMonitor.ShouldUseForwardTracker())
{
// get vehicle parameterization
TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final);
parameterizations.Add(vehicleParameters);
}
// sort and return funal
parameterizations.Sort();
// get the final behavior
TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior;
// get vehicles to ignore
tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore;
// add persistent information about saudi level
if (turnState.Saudi == SAUDILevel.L1)
{
tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray());
tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1));
}
// add persistent information about turn bounds
if (!turnState.UseTurnBounds)
{
tb.LeftBound = null;
tb.RightBound = null;
}
// return the behavior
return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp));
}
#endregion
#region Final is Zone Waypoint
else if (turn.FinalGeneric is ArbiterPerimeterWaypoint)
{
// get inteconnect path
Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center -
turn.FinalGeneric.Position;
entryVec = entryVec.Normalize(TahoeParams.VL / 2.0);
LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position });
// get distance from end
double d = ip.DistanceBetween(
ip.GetClosestPoint(vehicleState.Front),
ip.EndPoint);
// get speed command
SpeedCommand sc = null;
if (d < TahoeParams.VL)
{
sc = new StopAtDistSpeedCommand(d);
}
else
{
sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed));
}
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL);
// hold brake
IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc);
TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId);
// add persistent information about saudi level
if (turnState.Saudi == SAUDILevel.L1)
{
tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray());
tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1));
}
// add persistent information about turn bounds
if (!turnState.UseTurnBounds)
{
tb.LeftBound = null;
tb.RightBound = null;
}
// return maneuver
return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp));
}
#endregion
#region Unknown
else
{
throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString());
}
#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
}
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;
}
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
IList <LinePath> leftBounds, IList <LinePath> rightBounds,
double maxSpeed, double?endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double?approachSpeed)
{
// if we're in listen mode, just return for now
if (OperationalBuilder.BuildMode == BuildMode.Listen)
{
return;
}
PathPlanner.PlanningResult result;
double curSpeed = Services.StateProvider.GetVehicleState().speed;
LinePath targetPath = new LinePath();
double initialHeading = 0;
// get the part we just used to make a prediction
if (useTargetPath && previousSmoothedPath != null)
{
//targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
// interpolate the path with a smoothing spline
//targetPath = targetPath.SplineInterpolate(0.05);
//Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
// calculate the point speed*dt ahead
/*double lookaheadDist = curSpeed*0.20;
* if (lookaheadDist > 0.1) {
* LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
* // get the heading
* initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
* // adjust the base path start point to be the predicted location
* basePath[0] = pt.Location;
*
* // get the part we just used to make a prediction
* predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
* // push to the UI
* Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
* //basePath[0] = new Coordinates(lookaheadDist, 0);
* Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
* Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
* // calculate a piece of the sub path
* //targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
* }*/
// get the tracking manager to predict stuff like whoa
AbsolutePose absPose;
OperationalVehicleState vehicleState;
Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
// insert the stuff stuff
basePath[0] = absPose.xy;
initialHeading = absPose.heading;
// start walking down the path until the angle is cool
double angle_threshold = 30 * Math.PI / 180.0;
double dist;
LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
for (dist = 0; dist < 10; dist += 1)
{
// get the point advanced from the 2nd point on the base path by dist
double distTemp = dist;
newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);
// check if we're past the end
if (distTemp > 0)
{
break;
}
// check if the angle is coolness or not
double angle = Math.Acos((newPoint.Location - basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));
if (Math.Acos(angle) < angle_threshold)
{
break;
}
}
// create a new version of the base path with the stuff section removed
basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);
Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
// zero that stuff out
targetPath = new LinePath();
}
StaticObstacles obstacles = null;
// only do the planning is we're in a lane scenario
// otherwise, the obstacle grid will be WAY too large
if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1)
{
// get the obstacles predicted to the current timestamp
obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
}
// start the planning timer
Stopwatch planningTimer = Stopwatch.StartNew();
// check if there are any obstacles
if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0)
{
if (cancelled)
{
return;
}
// we need to do the full obstacle avoidance
// execute the obstacle manager
LinePath avoidancePath;
List <ObstacleManager.ObstacleType> obstacleSideFlags;
bool success;
Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
out avoidancePath, out obstacleSideFlags, out success);
// check if we have success
if (success)
{
// build the boundary lists
// start with the lanes
List <Boundary> leftSmootherBounds = new List <Boundary>();
List <Boundary> rightSmootherBounds = new List <Boundary>();
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.1;
double laneAlphaS = 0.1;
leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.8;
double obstacleDesiredSpacing = 0.8;
double obstacleAlphaS = 100;
int totalObstacleClusters = obstacles.polygons.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left)
{
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
leftSmootherBounds.Add(bound);
}
else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right)
{
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
rightSmootherBounds.Add(bound);
}
}
if (cancelled)
{
return;
}
// execute the smoothing
PathPlanner planner = new PathPlanner();
planner.Options.alpha_w = 0;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
else
{
// mark that we did not succeed
result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
}
}
else
{
if (cancelled)
{
return;
}
// do the path smoothing
PathPlanner planner = new PathPlanner();
List <LineList> leftList = new List <LineList>();
foreach (LinePath ll in leftBounds)
{
leftList.Add(ll);
}
List <LineList> rightList = new List <LineList>();
foreach (LinePath rl in rightBounds)
{
rightList.Add(rl);
}
planner.Options.alpha_w = 0;
planner.Options.alpha_s = 0.1;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
planningTimer.Stop();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);
Services.Dataset.ItemAs <bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);
if (result.result == SmoothResult.Sucess)
{
// insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
//Coordinates startingVec = result.path[1].Point-result.path[0].Point;
//Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
//result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));
previousSmoothedPath = new LinePath(result.path);
previousSmoothedPathTimestamp = curTimestamp;
Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);
if (cancelled)
{
return;
}
// we've planned out the path, now build up the command
ISpeedGenerator speedGenerator;
if (speedCommand is ScalarSpeedCommand)
{
/*if (result.path.HasSpeeds) {
* speedGenerator = result.path;
* }
* else {*/
speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
//}
}
else if (speedCommand is StopAtDistSpeedCommand)
{
StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand is StopAtLineSpeedCommand)
{
IDistanceProvider distProvider = new StoplineDistanceProvider();
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand == null)
{
throw new InvalidOperationException("Speed command is null");
}
else
{
throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
}
if (cancelled)
{
return;
}
// build up the command
TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
trackingCommand.Label = commandLabel;
// queue it to execute
Services.TrackingManager.QueueCommand(trackingCommand);
}
}
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;
}
}
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;
}
}
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);
}
public Game(string localPlayerName, string oponentPlayerName, string gameId)
{
_gameId = gameId;
PlayerOne.Name = localPlayerName;
PlayerTwo.Name = oponentPlayerName;
DataContext = PlayerOne;
debug.DataContext = PlayerOne;
InitializeComponent();
PlayerOne.GameOver += OnGameOver;
Background_Canvas.Children.RemoveRange(3, Background_Canvas.Children.Count);
player2.DataContext = PlayerTwo;
counterOponent.DataContext = PlayerTwo;
//debug.Show();
//get game hub connection and register methods
_gameHubConnection = GameHubConnection.GetInstance();
_gameHubConnection.RegisterMethod("UpdateOponent", new Action <Object>((Object playerRequest) =>
{
Dispatcher.Invoke((Action)(async() =>
{
var player = JsonConvert.DeserializeObject <PlayerDto>(playerRequest.ToString());
await UpdateOponent(player);
}));
}
));
_gameHubConnection.RegisterMethod("DeleteItem", new Action <Object>((Object itemRequest) =>
{
Dispatcher.Invoke((Action)(async() =>
{
var item = JsonConvert.DeserializeObject <ItemDto>(itemRequest.ToString());
await DeleteItem(item);
}));
}
));
_gameHubConnection.RegisterMethod("EndGame", new Action <GameEndDto>((GameEndDto request) =>
{
Dispatcher.Invoke((Action)(async() =>
{
await EndGame(request);
}));
}
));
//building map
//var builder = new MapBuilder(new EasyGameObjectFactory());
var builder = new MapBuilder(new MediumGameObjectFactory(), Background_Canvas);
var director = new MapDirector(builder);
director.ConstructMap();
_items = builder.GetMap();
//setting up command and invoker
Models.Commands.ICommand command = new SpeedCommand(PlayerOne);
_invoker = new Invoker();
_invoker.SetCommand(command);
//sound adapter
_soundAdapter = new NullObjectSoundAdapter();
_soundAdapter.Play();
//animation adapter
_playerOneAnimationAdapter = new PlayerAnimationAdapter(Player_Canvas);
_playerTwoAnimationAdapter = new PlayerAnimationAdapter(player2);
_starAnimationAdapter = new ObjectAnimationAdapter(new StarAnimationController(Background_Canvas));
_starShieldAnimationAdapter = new ObjectAnimationAdapter(new StarShieldAnimationController(Background_Canvas));
//prototype
//TestingDeepAndShallowCopies();
//Interpreter
_expressionParser = new ExpressionParser();
//Memento
_careTaker = new CareTaker();
GraphicalEffects.EndTransiton(Background_Canvas, GamePage);
PhysicsTimer = new DispatcherTimer();
PhysicsTimer.Tick += PhysicsTimerTick;
PhysicsTimer.Interval = new TimeSpan(0, 0, 0, 0, 10);
PhysicsTimer.Start();
//task1 = new Task(PhysicsTimer.Start);
AnimationTimer = new DispatcherTimer();
AnimationTimer.Tick += AnimationTimerTick;
AnimationTimer.Interval = new TimeSpan(0, 0, 0, 0, 50);
AnimationTimer.Start();
//task2 = new Task(AnimationTimer.Start);
StarTimer = new DispatcherTimer();
StarTimer.Tick += StarTimerTick;
StarTimer.Interval = new TimeSpan(0, 0, 0, 0, 50);
StarTimer.Start();
//task3 = new Task(CoinTimer.Start);
JumpingTimer = new DispatcherTimer();
JumpingTimer.Tick += JumpingTimerTick;
JumpingTimer.Interval = new TimeSpan(0, 0, 0, 0, 400);
JumpingTimer.Start();
//task4 = new Task(JumpingTimer.Start);
NetworkTimer = new DispatcherTimer();
NetworkTimer.Tick += NetworktTimerTick;
NetworkTimer.Interval = new TimeSpan(0, 0, 0, 0, 20);
NetworkTimer.Start();
//task5 = new Task(NetworktTimer.Start);
//task1.Start();
//task2.Start();
//task3.Start();
//task4.Start();
//task5.Start();
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
IList<LinePath> leftBounds, IList<LinePath> rightBounds,
double maxSpeed, double? endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double? approachSpeed)
{
// if we're in listen mode, just return for now
if (OperationalBuilder.BuildMode == BuildMode.Listen) {
return;
}
PathPlanner.PlanningResult result;
double curSpeed = Services.StateProvider.GetVehicleState().speed;
LinePath targetPath = new LinePath();
double initialHeading = 0;
// get the part we just used to make a prediction
if (useTargetPath && previousSmoothedPath != null) {
//targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
// interpolate the path with a smoothing spline
//targetPath = targetPath.SplineInterpolate(0.05);
//Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
// calculate the point speed*dt ahead
/*double lookaheadDist = curSpeed*0.20;
if (lookaheadDist > 0.1) {
LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
// get the heading
initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
// adjust the base path start point to be the predicted location
basePath[0] = pt.Location;
// get the part we just used to make a prediction
predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
// push to the UI
Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
//basePath[0] = new Coordinates(lookaheadDist, 0);
Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
// calculate a piece of the sub path
//targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
}*/
// get the tracking manager to predict stuff like whoa
AbsolutePose absPose;
OperationalVehicleState vehicleState;
Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
// insert the stuff stuff
basePath[0] = absPose.xy;
initialHeading = absPose.heading;
// start walking down the path until the angle is cool
double angle_threshold = 30*Math.PI/180.0;
double dist;
LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
for (dist = 0; dist < 10; dist += 1) {
// get the point advanced from the 2nd point on the base path by dist
double distTemp = dist;
newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);
// check if we're past the end
if (distTemp > 0) {
break;
}
// check if the angle is coolness or not
double angle = Math.Acos((newPoint.Location-basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));
if (Math.Acos(angle) < angle_threshold) {
break;
}
}
// create a new version of the base path with the stuff section removed
basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);
Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
// zero that stuff out
targetPath = new LinePath();
}
StaticObstacles obstacles = null;
// only do the planning is we're in a lane scenario
// otherwise, the obstacle grid will be WAY too large
if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1) {
// get the obstacles predicted to the current timestamp
obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
}
// start the planning timer
Stopwatch planningTimer = Stopwatch.StartNew();
// check if there are any obstacles
if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0) {
if (cancelled) return;
// we need to do the full obstacle avoidance
// execute the obstacle manager
LinePath avoidancePath;
List<ObstacleManager.ObstacleType> obstacleSideFlags;
bool success;
Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
out avoidancePath, out obstacleSideFlags, out success);
// check if we have success
if (success) {
// build the boundary lists
// start with the lanes
List<Boundary> leftSmootherBounds = new List<Boundary>();
List<Boundary> rightSmootherBounds = new List<Boundary>();
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.1;
double laneAlphaS = 0.1;
leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.8;
double obstacleDesiredSpacing = 0.8;
double obstacleAlphaS = 100;
int totalObstacleClusters = obstacles.polygons.Count;
for (int i = 0; i < totalObstacleClusters; i++) {
if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left) {
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
leftSmootherBounds.Add(bound);
}
else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right) {
Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
bound.CheckFrontBumper = true;
rightSmootherBounds.Add(bound);
}
}
if (cancelled) return;
// execute the smoothing
PathPlanner planner = new PathPlanner();
planner.Options.alpha_w = 0;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
else {
// mark that we did not succeed
result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
}
}
else {
if (cancelled) return;
// do the path smoothing
PathPlanner planner = new PathPlanner();
List<LineList> leftList = new List<LineList>();
foreach (LinePath ll in leftBounds) leftList.Add(ll);
List<LineList> rightList = new List<LineList>();
foreach (LinePath rl in rightBounds) rightList.Add(rl);
planner.Options.alpha_w = 0;
planner.Options.alpha_s = 0.1;
planner.Options.alpha_d = 10;
planner.Options.alpha_c = 10;
result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
}
planningTimer.Stop();
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);
Services.Dataset.ItemAs<bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);
if (result.result == SmoothResult.Sucess) {
// insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
//Coordinates startingVec = result.path[1].Point-result.path[0].Point;
//Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
//result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));
previousSmoothedPath = new LinePath(result.path);
previousSmoothedPathTimestamp = curTimestamp;
Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);
if (cancelled) return;
// we've planned out the path, now build up the command
ISpeedGenerator speedGenerator;
if (speedCommand is ScalarSpeedCommand) {
/*if (result.path.HasSpeeds) {
speedGenerator = result.path;
}
else {*/
speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
//}
}
else if (speedCommand is StopAtDistSpeedCommand) {
StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand is StopAtLineSpeedCommand) {
IDistanceProvider distProvider = new StoplineDistanceProvider();
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand == null) {
throw new InvalidOperationException("Speed command is null");
}
else {
throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
}
if (cancelled) return;
// build up the command
TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
trackingCommand.Label = commandLabel;
// queue it to execute
Services.TrackingManager.QueueCommand(trackingCommand);
}
}
public static void SmoothAndTrack(LinePath basePath, bool useTargetPath,
LinePath leftBound, LinePath rightBound,
double maxSpeed, double? endingHeading, bool endingOffsetBound,
CarTimestamp curTimestamp, bool doAvoidance,
SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
string commandLabel, ref bool cancelled,
ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double? approachSpeed)
{
SmoothAndTrack(basePath, useTargetPath, new LinePath[] { leftBound }, new LinePath[] { rightBound }, maxSpeed, endingHeading, endingOffsetBound, curTimestamp, doAvoidance, speedCommand, behaviorTimestamp, commandLabel, ref cancelled, ref previousSmoothedPath, ref previousSmoothedPathTimestamp, ref approachSpeed);
}
