public void PushAbsolutePose(AbsolutePose pose)
{
lock (lockobj) {
queue.Add(pose);
if (!queue.VerifySort(delegate(AbsolutePose l, AbsolutePose r) { return l.timestamp.CompareTo(r.timestamp); })) {
OperationalTrace.WriteError("absolute sort is donzoed, flushing queue");
queue.Clear();
}
}
}
void client_PoseAbsReceived(object sender, PoseAbsReceivedEventArgs e)
{
packetCount++;
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
//OperationalTrace.WriteVerbose("got absolute pose for time {0}", e.PoseAbsData.timestamp);
if (lastPacketTime.ts > e.PoseAbsData.timestamp.ts)
{
lastPacketTime = new CarTimestamp(-10000);
}
if (e.PoseAbsData.timestamp.ts - lastPacketTime.ts >= 0.02)
{
DatasetSource ds = Services.Dataset;
PoseAbsData d = e.PoseAbsData;
CarTimestamp now = e.PoseAbsData.timestamp;
if (Services.Projection != null)
{
Coordinates xy = Services.Projection.ECEFtoXY(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <Coordinates>("xy").Add(xy, now);
AbsolutePose absPose = new AbsolutePose(xy, d.yaw, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
if (!Settings.UseWheelSpeed)
{
ds.ItemAs <double>("speed").Add(d.veh_vx, now);
}
ds.ItemAs <double>("vel - y").Add(d.veh_vy, now);
ds.ItemAs <double>("heading").Add(BiasEstimator.CorrectHeading(d.yaw), now);
ds.ItemAs <double>("pitch").Add(d.pitch + 1.25 * Math.PI / 180.0, now);
ds.ItemAs <double>("roll").Add(d.roll, now);
ds.ItemAs <double>("ba - x").Add(d.bax, now);
ds.ItemAs <double>("ba - y").Add(d.bay, now);
ds.ItemAs <double>("ba - z").Add(d.baz, now);
ds.ItemAs <double>("bw - x").Add(d.bwx, now);
ds.ItemAs <double>("bw - y").Add(d.bwy, now);
ds.ItemAs <double>("bw - z").Add(d.bwz, now);
ds.ItemAs <PoseCorrectionMode>("correction mode").Add(d.correction_mode, now);
LLACoord lla = WGS84.ECEFtoLLA(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <double>("altitude").Add(lla.alt, now);
if (Services.Projection != null)
{
ds.ItemAs <Coordinates>("gps xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.gps_px, d.gps_py, d.gps_pz)), now);
ds.ItemAs <Coordinates>("hp xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.hp_px, d.hp_py, d.hp_pz)), now);
}
ds.ItemAs <double>("sep heading").Add(d.sep_heading, now);
ds.ItemAs <double>("sep pitch").Add(d.sep_pitch, now);
ds.ItemAs <double>("sep roll").Add(d.sep_roll, now);
if (!double.IsNaN(lastYawRate) && lastPacketTime.ts > 0)
{
// get the enu velocity
Vector3 pECEF = new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz);
Vector3 vECEF = new Vector3(d.ecef_vx, d.ecef_vy, d.ecef_vz);
Matrix3 Recef2enu = Geocentric.Recef2enu(pECEF);
Vector3 vENU = Recef2enu * vECEF;
BiasEstimator.Update(lastYawRate, d.yaw, vENU, now.ts - lastPacketTime.ts, now);
Services.Dataset.ItemAs <double>("heading bias").Add(BiasEstimator.HeadingBias, now);
}
lastPacketTime = now;
}
}
/// <summary>
/// Predicts forward by the delay time of the tahoe
/// </summary>
/// <param name="absPose"></param>
/// <param name="vehicleState"></param>
public void ForwardPredict(double planningTime, out AbsolutePose absPose, out OperationalVehicleState vehicleState)
{
// get the current state
OperationalVehicleState currentState = Services.StateProvider.GetVehicleState();
// assume the vehicle will hold it current curvature
double curvature = TahoeParams.CalculateCurvature(currentState.steeringAngle, currentState.speed);
// simple dynamics
// xdot = v*cos(theta)
// ydot = v*sin(theta)
// thetadot = v*c
// do euler integration
double dt = 0.01;
double T = TahoeParams.actuation_delay + planningTime;
int n = (int)Math.Round(T/dt);
double v = currentState.speed;
double x = 0, y = 0, heading = 0;
for (int i = 0; i < n; i++) {
x += v*Math.Cos(heading)*dt;
y += v*Math.Sin(heading)*dt;
heading += v*curvature*dt;
}
absPose = new AbsolutePose(new Coordinates(x, y), heading, 0);
vehicleState = currentState;
}
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 AbsoluteTransformer(AbsolutePose pose) : this(pose.xy, pose.heading, pose.timestamp)
{
}
public AbsoluteTransformer(AbsolutePose pose)
: this(pose.xy, pose.heading, pose.timestamp)
{
}
void client_PoseAbsReceived(object sender, PoseAbsReceivedEventArgs e)
{
packetCount++;
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
//OperationalTrace.WriteVerbose("got absolute pose for time {0}", e.PoseAbsData.timestamp);
if (lastPacketTime.ts > e.PoseAbsData.timestamp.ts) {
lastPacketTime = new CarTimestamp(-10000);
}
if (e.PoseAbsData.timestamp.ts - lastPacketTime.ts >= 0.02) {
DatasetSource ds = Services.Dataset;
PoseAbsData d = e.PoseAbsData;
CarTimestamp now = e.PoseAbsData.timestamp;
if (Services.Projection != null) {
Coordinates xy = Services.Projection.ECEFtoXY(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs<Coordinates>("xy").Add(xy, now);
AbsolutePose absPose = new AbsolutePose(xy, d.yaw, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
if (!Settings.UseWheelSpeed) {
ds.ItemAs<double>("speed").Add(d.veh_vx, now);
}
ds.ItemAs<double>("vel - y").Add(d.veh_vy, now);
ds.ItemAs<double>("heading").Add(BiasEstimator.CorrectHeading(d.yaw), now);
ds.ItemAs<double>("pitch").Add(d.pitch + 1.25 * Math.PI/180.0, now);
ds.ItemAs<double>("roll").Add(d.roll, now);
ds.ItemAs<double>("ba - x").Add(d.bax, now);
ds.ItemAs<double>("ba - y").Add(d.bay, now);
ds.ItemAs<double>("ba - z").Add(d.baz, now);
ds.ItemAs<double>("bw - x").Add(d.bwx, now);
ds.ItemAs<double>("bw - y").Add(d.bwy, now);
ds.ItemAs<double>("bw - z").Add(d.bwz, now);
ds.ItemAs<PoseCorrectionMode>("correction mode").Add(d.correction_mode, now);
LLACoord lla = WGS84.ECEFtoLLA(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs<double>("altitude").Add(lla.alt, now);
if (Services.Projection != null) {
ds.ItemAs<Coordinates>("gps xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.gps_px, d.gps_py, d.gps_pz)), now);
ds.ItemAs<Coordinates>("hp xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.hp_px, d.hp_py, d.hp_pz)), now);
}
ds.ItemAs<double>("sep heading").Add(d.sep_heading, now);
ds.ItemAs<double>("sep pitch").Add(d.sep_pitch, now);
ds.ItemAs<double>("sep roll").Add(d.sep_roll, now);
if (!double.IsNaN(lastYawRate) && lastPacketTime.ts > 0) {
// get the enu velocity
Vector3 pECEF = new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz);
Vector3 vECEF = new Vector3(d.ecef_vx, d.ecef_vy, d.ecef_vz);
Matrix3 Recef2enu = Geocentric.Recef2enu(pECEF);
Vector3 vENU = Recef2enu*vECEF;
BiasEstimator.Update(lastYawRate, d.yaw, vENU, now.ts-lastPacketTime.ts, now);
Services.Dataset.ItemAs<double>("heading bias").Add(BiasEstimator.HeadingBias, now);
}
lastPacketTime = now;
}
}
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);
}
}