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); } }