private void ExecutePass()
{
// determine what to do
// get the moving order
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
curTimestamp = pose.timestamp;
ParkerVehicleState state = new ParkerVehicleState(pose.xy, Coordinates.FromAngle(pose.heading));
if (pulloutMode)
{
movingOrder = parker.GetNextPulloutOrder(state);
}
else
{
movingOrder = parker.GetNextParkingOrder(state);
}
// test for completion
bool isPullinCompleted = false;
if (parkingSpotLine.P0.DistanceTo(pose.xy) < 1.5 && Coordinates.FromAngle(pose.heading).Dot(parkingSpotLine.UnitVector) >= 0.5)
{
isPullinCompleted = true;
}
finalPass = false;
if (movingOrder != null && movingOrder.DestState != null && parkingSpotLine.P0.DistanceTo(movingOrder.DestState.Loc) < 1.5 && parkingSpotLine.UnitVector.Dot(movingOrder.DestState.Heading) >= 0.5)
{
finalPass = true;
}
if (movingOrder.Completed || (isPullinCompleted && !pulloutMode))
{
Type type;
if (pulloutMode)
{
type = typeof(ZoneParkingPullOutBehavior);
}
else
{
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
}
else
{
// determine the stopping distance
TransmissionGear targetGear;
if (movingOrder.Forward)
{
targetGear = TransmissionGear.First;
}
else
{
targetGear = TransmissionGear.Reverse;
}
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0))
{
counterClockwise = true;
}
else
{
counterClockwise = false;
}
double radius = pose.xy.DistanceTo(movingOrder.CenterPoint);
double startAngle = (pose.xy - movingOrder.CenterPoint).ArcTan;
double endAngle = (movingOrder.DestState.Loc - movingOrder.CenterPoint).ArcTan;
if (counterClockwise)
{
if (endAngle < startAngle)
{
endAngle += 2 * Math.PI;
}
}
else
{
if (endAngle > startAngle)
{
endAngle -= 2 * Math.PI;
}
}
double stopDistance;
if (Math.Abs(radius) < 100)
{
stopDistance = Math.Abs(radius * (endAngle - startAngle));
}
else
{
stopDistance = pose.xy.DistanceTo(movingOrder.DestState.Loc);
}
this.stopDist = stopDistance;
this.stopTimestamp = curTimestamp;
Services.UIService.PushPoint(movingOrder.DestState.Loc, curTimestamp, "uturn stop point", false);
//double sign = movingOrder.Forward ? 1 : -1;
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / movingOrder.TurningRadius, parking_speed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(targetGear);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), parking_speed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = command_label;
Services.TrackingManager.QueueCommand(cmd);
}
passCompleted = false;
}
public void MessageArrived(string channelName, object message)
{
// check the method
if (message is LocalRoadEstimate)
{
LocalRoadEstimate lre = (LocalRoadEstimate)message;
CarTimestamp ct = lre.timestamp;
// get the stop-line estimate
Services.Dataset.ItemAs <bool>("stopline found").Add(lre.stopLineEstimate.stopLineExists, ct);
Services.Dataset.ItemAs <double>("stopline distance").Add(lre.stopLineEstimate.distToStopline, ct);
Services.Dataset.ItemAs <double>("stopline variance").Add(lre.stopLineEstimate.distToStoplineVar, ct);
}
else if (message is PathRoadModel && Settings.UsePathRoadModel)
{
PathRoadModel pathRoadModel = (PathRoadModel)message;
List <ILaneModel> models = new List <ILaneModel>();
foreach (PathRoadModel.LaneEstimate le in pathRoadModel.laneEstimates)
{
models.Add(new PathLaneModel(pathRoadModel.timestamp, le));
}
Services.LaneModels.SetLaneModel(models);
}
else if (message is VehicleState)
{
VehicleState vs = (VehicleState)message;
AbsolutePose pose = new AbsolutePose(vs.Position, vs.Heading.ArcTan, vs.Timestamp);
Services.AbsolutePosteriorPose.PushAbsolutePose(pose);
Services.Dataset.ItemAs <Coordinates>("posterior pose").Add(vs.Position, vs.Timestamp);
double heading = vs.Heading.ArcTan;
if (Services.PoseListener != null && Services.PoseListener.BiasEstimator != null)
{
heading = Services.PoseListener.BiasEstimator.CorrectHeading(heading);
}
Services.Dataset.ItemAs <double>("posterior heading").Add(heading, vs.Timestamp);
TimeoutMonitor.MarkData(OperationalDataSource.PosteriorPose);
}
else if (message is SceneEstimatorUntrackedClusterCollection)
{
// push to the obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived((SceneEstimatorUntrackedClusterCollection)message);
TimeoutMonitor.MarkData(OperationalDataSource.UntrackedClusters);
}
else if (message is SceneEstimatorTrackedClusterCollection)
{
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
TimeoutMonitor.MarkData(OperationalDataSource.TrackedClusters);
}
//else if (message is LocalRoadModel) {
// LocalRoadModel roadModel = (LocalRoadModel)message;
// UrbanChallenge.Operational.Common.LocalLaneModel centerLaneModel =
// new UrbanChallenge.Operational.Common.LocalLaneModel(
// new LinePath(roadModel.LanePointsCenter), ArrayConvert(roadModel.LanePointsCenterVariance), roadModel.laneWidthCenter,
// roadModel.laneWidthCenterVariance, roadModel.probabilityCenterLaneExists);
// UrbanChallenge.Operational.Common.LocalLaneModel leftLaneModel =
// new UrbanChallenge.Operational.Common.LocalLaneModel(
// new LinePath(roadModel.LanePointsLeft), ArrayConvert(roadModel.LanePointsLeftVariance), roadModel.laneWidthLeft,
// roadModel.laneWidthLeftVariance, roadModel.probabilityLeftLaneExists);
// UrbanChallenge.Operational.Common.LocalLaneModel rightLaneModel =
// new UrbanChallenge.Operational.Common.LocalLaneModel(
// new LinePath(roadModel.LanePointsRight), ArrayConvert(roadModel.LanePointsRightVariance), roadModel.laneWidthRight,
// roadModel.laneWidthRightVariance, roadModel.probabilityRightLaneExists);
// UrbanChallenge.Operational.Common.LocalRoadModel localRoadModel = new UrbanChallenge.Operational.Common.LocalRoadModel(
// roadModel.timestamp, roadModel.probabilityRoadModelValid, centerLaneModel, leftLaneModel, rightLaneModel);
// //Services.RoadModelProvider.LocalRoadModel = localRoadModel;
// // clone the lane models so when we transform to absolute coordinates for the ui, the lane model we have
// // stored doesn't get modified
// centerLaneModel = centerLaneModel.Clone();
// leftLaneModel = leftLaneModel.Clone();
// rightLaneModel = rightLaneModel.Clone();
// AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(roadModel.timestamp).Invert();
// centerLaneModel.LanePath.TransformInPlace(trans);
// leftLaneModel.LanePath.TransformInPlace(trans);
// rightLaneModel.LanePath.TransformInPlace(trans);
// // send to ui
// Services.Dataset.ItemAs<UrbanChallenge.Operational.Common.LocalLaneModel>("center lane").Add(centerLaneModel, roadModel.timestamp);
// Services.Dataset.ItemAs<UrbanChallenge.Operational.Common.LocalLaneModel>("left lane").Add(leftLaneModel, roadModel.timestamp);
// Services.Dataset.ItemAs<UrbanChallenge.Operational.Common.LocalLaneModel>("right lane").Add(rightLaneModel, roadModel.timestamp);
// Services.Dataset.ItemAs<double>("lane model probability").Add(roadModel.probabilityRoadModelValid, roadModel.timestamp);
//}
else if (message is SideObstacles)
{
Services.ObstaclePipeline.OnSideObstaclesReceived((SideObstacles)message);
}
else if (message is SideRoadEdge)
{
SideRoadEdge edge = (SideRoadEdge)message;
RoadEdge.OnRoadEdge(edge);
LineList line = RoadEdge.GetEdgeLine(edge);
if (line == null)
{
line = new LineList();
}
string name = (edge.side == SideRoadEdgeSide.Driver) ? "left road edge" : "right road edge";
Services.UIService.PushLineList(line, edge.timestamp, name, true);
}
else if (message is SparseRoadBearing)
{
SparseRoadBearing road = (SparseRoadBearing)message;
RoadBearing.OnRoadBearing(road.timestamp, road.Heading, road.Confidence);
}
}
public override void Process(object param)
{
// inform the base that we're beginning processing
if (!BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is StayInLaneBehavior)
{
HandleBehavior((StayInLaneBehavior)param);
}
if (reverseGear)
{
ProcessReverse();
return;
}
// figure out the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10)
{
planningDistance = 10;
}
double?boundStartDistMin = null;
double?boundEndDistMax = null;
if (laneID != null && Services.RoadNetwork != null)
{
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy);
LinePath partitionPath = partition.UserPartitionPath;
LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy);
double remainingDist = planningDistance;
double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint);
remainingDist -= totalDist;
if (sparse)
{
// walk ahead and determine where sparsity ends
bool nonSparseFound = false;
while (remainingDist > 0)
{
// get the next partition
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type != PartitionType.Sparse)
{
nonSparseFound = true;
break;
}
else
{
double dist = partition.UserPartitionPath.PathLength;
totalDist += dist;
remainingDist -= dist;
}
}
if (nonSparseFound)
{
boundStartDistMin = totalDist;
}
}
else
{
// determine if there is a sparse segment upcoming
bool sparseFound = false;
while (remainingDist > 0)
{
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type == PartitionType.Sparse)
{
sparseFound = true;
break;
}
else
{
double dist = partition.Length;
totalDist += dist;
remainingDist -= dist;
}
}
if (sparseFound)
{
boundEndDistMax = totalDist;
sparse = true;
}
}
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance);
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight);
double avoidanceExtra = sparse ? 5 : 7.5;
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null)
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse)
{
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists)
{
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists)
{
double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5)
{
// shift to the right
double shiftDist = -TahoeParams.T / 2.0;
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null)
{
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists)
{
AddRightBound(rightBound, false);
}
}
double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint;
double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength;
extraDist = Math.Min(extraDist, 5);
if (extraDist > 0)
{
centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist));
}
avoidanceBasePath = centerLine;
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
// get the overall max speed looking forward from our current point
settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay));
// get the max speed at the end point
settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance));
useAvoidancePath = false;
if (sparse)
{
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null)
{
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null)
{
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null)
{
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else
{
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
return;
}
else if (oncomingVehicleExists)
{
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
disablePathAngleCheck = false;
SmoothAndTrack(commandLabel, true);
}
void IChannelListener.MessageArrived(string channelName, object message)
{
if (message is OperationalSimVehicleState)
{
OperationalTrace.ThreadTraceSource = TraceSource;
Trace.CorrelationManager.StartLogicalOperation("simstate callback");
try {
OperationalSimVehicleState state = (OperationalSimVehicleState)message;
DatasetSource ds = Services.Dataset;
OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp);
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
CarTimestamp now = state.Timestamp;
ds.ItemAs <Coordinates>("xy").Add(state.Position, now);
ds.ItemAs <double>("speed").Add(state.Speed, now);
ds.ItemAs <double>("heading").Add(state.Heading, now);
ds.ItemAs <double>("actual steering").Add(state.SteeringAngle, now);
ds.ItemAs <TransmissionGear>("transmission gear").Add(state.TransmissionGear, now);
ds.ItemAs <double>("engine torque").Add(state.EngineTorque, now);
ds.ItemAs <double>("brake pressure").Add(state.BrakePressure, now);
ds.ItemAs <double>("rpm").Add(state.EngineRPM, now);
if (state.TransmissionGear != lastRecvTransGear)
{
ds.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, now);
lastRecvTransGear = state.TransmissionGear;
}
lastCarMode = state.CarMode;
if (CarModeChanged != null)
{
CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now));
}
// build the current relative pose matrix
Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0) * Matrix4.YPR(state.Heading, 0, 0);
relativePose = relativePose.Inverse();
// push on to the relative pose stack
Services.RelativePose.PushTransform(now, relativePose);
// push the current absolute pose entry
AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
OperationalTrace.ThreadTraceSource = null;
}
}
else if (message is SceneEstimatorUntrackedClusterCollection)
{
// push to obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message));
}
else if (message is SceneEstimatorTrackedClusterCollection)
{
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
}
}
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;
}
}
