public void HandleMessage(byte[] message, object target, DatasetSource ds) { BigEndianBinaryReader reader = new BigEndianBinaryReader(message); // read in the current time CarTimestamp time = new CarTimestamp(reader.ReadUInt16(), reader.ReadInt32()); // read the feedback type byte feedbackType = reader.ReadByte(); // read the length ushort len = reader.ReadUInt16(); // dispatch if we can FeedbackType type; if (types.TryGetValue((int)feedbackType, out type)) { object[] vals = type.MapMessage(target, ds, time, reader); if (logWriter != null) { logWriter.Write(time); logWriter.Write(","); logWriter.Write(feedbackType); logWriter.Write(","); for (int i = 0; i < vals.Length - 1; i++) { logWriter.Write(vals[i]); logWriter.Write(","); } logWriter.WriteLine(vals[vals.Length - 1]); } } }
public OperationalVehicleState GetVehicleState() { DatasetSource ds = Services.Dataset; return(new OperationalVehicleState( ds.ItemAs <double>("speed").CurrentValue, ds.ItemAs <double>("actual steering").CurrentValue, ds.ItemAs <TransmissionGear>("transmission gear").CurrentValue, ds.ItemAs <double>("pitch").CurrentValue, ds.ItemAs <double>("brake pressure").CurrentValue, ds.ItemAs <double>("engine torque").CurrentValue, ds.ItemAs <double>("rpm").CurrentValue, ds.ItemAs <double>("pedal position").CurrentValue, ds.ItemAs <double>("actual steering").CurrentTime)); }
public object[] MapMessage(object target, DatasetSource ds, CarTimestamp ts, BigEndianBinaryReader reader) { // reader will be positioned at start of payload int nextra = 0; if (methodHasTimestamp) nextra++; if (methodHasMsgType) nextra++; object[] vals = new object[fields.Count]; object[] param = null; if (method != null) { if (methodIsObjectArray) { param = new object[1+nextra]; } else { param = new object[fields.Count + nextra]; } if (methodHasTimestamp) { param[0] = ts; if (methodHasMsgType) param[1] = id; } else if (methodHasMsgType) { param[0] = id; } } for (int i = 0; i < fields.Count; i++) { object val = fields[i].MapField(reader, ds, ts); if (param != null && !methodIsObjectArray) param[i + nextra] = val; vals[i] = val; } if (methodIsObjectArray && param != null) { param[nextra] = vals; } if (method != null) { method.Invoke(target, param); } return vals; }
public object MapField(BigEndianBinaryReader reader, DatasetSource ds, CarTimestamp ct) { object val = MapData(ReadData(reader)); if (!string.IsNullOrEmpty(dsitem) && ds != null) { IDataItemSource di; if (!ds.TryGetValue(dsitem, out di)) { Type digType = typeof(DataItemSource <>); Type diType = digType.MakeGenericType(outtype); DataItemDescriptor desc = new DataItemDescriptor(dsitem, outtype, null, null, 250); di = (IDataItemSource)Activator.CreateInstance(diType, desc); ds.Add(dsitem, di); } di.Add(val, ct); } return(val); }
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; } }
public static TestResult IsValidDataset(this string filePath) { if (!File.Exists(filePath)) { return(Create.TestResult(TestStatus.Pass)); } string documentationLink = "IsValidDataset"; //Read the dataset StreamReader sr = new StreamReader(filePath); string json = sr.ReadToEnd(); sr.Close(); if (json == null) { return(Create.TestResult(TestStatus.Pass)); } //Check if the dataset deserialises to a dataset object Dataset ds = BH.Engine.Serialiser.Convert.FromJson(json) as Dataset; if (ds == null) { //Dataset did not deserialise successfully return(Create.TestResult(TestStatus.Error, new List <Error>() { Create.Error("Dataset file did not deserialise into a BH.oM.Data.Library.Dataset object successfully. For more information see https://github.com/BHoM/documentation/wiki/IsValidDataset", Create.Location(filePath, Create.LineSpan(1, 1)), documentationLink, TestStatus.Error, "Dataset deserialisation error" ) })); } if (ds.SourceInformation == null) { //Source information is not set return(Create.TestResult(TestStatus.Warning, new List <Error>() { Create.Error("Dataset file does not contain any source information. For more information see https://github.com/BHoM/documentation/wiki/IsValidDataset", Create.Location(filePath, Create.LineSpan(1, 1)), documentationLink, TestStatus.Warning, "Dataset source error" ) })); } DatasetSource dss = new DatasetSource(); dss.Message = "Title: " + ds.SourceInformation.Title + System.Environment.NewLine + "Author: " + ds.SourceInformation.Author + System.Environment.NewLine + "Confidence: " + ds.SourceInformation.Confidence.ToString() + System.Environment.NewLine + "Version: " + ds.SourceInformation.Version + System.Environment.NewLine + "Source Link: " + ds.SourceInformation.SourceLink + System.Environment.NewLine + "Publisher: " + ds.SourceInformation.Publisher + System.Environment.NewLine + "Language: " + ds.SourceInformation.Language + System.Environment.NewLine + "Schema: " + ds.SourceInformation.Schema + System.Environment.NewLine + "Location: " + ds.SourceInformation.Location + System.Environment.NewLine + "Copyright: " + ds.SourceInformation.Copyright + System.Environment.NewLine + "Contributors: " + ds.SourceInformation.Contributors + System.Environment.NewLine + "Item Reference: " + ds.SourceInformation.ItemReference + System.Environment.NewLine; dss.Status = TestStatus.Pass; TestResult result = new TestResult() { Status = TestStatus.Pass }; result.Information.Add(dss); List <Error> errors = new List <Error>(); if (ds.SourceInformation.Author == null || ds.SourceInformation.Author == "") { //Source information does not contain an author errors.Add(Create.Error("Dataset file does not contain an author in the source information. For more information see https://github.com/BHoM/documentation/wiki/IsValidDataset", Create.Location(filePath, Create.LineSpan(1, 1)), documentationLink, TestStatus.Warning, "Dataset source author error" )); result.Status = TestStatus.Warning; } if (ds.SourceInformation.Title == null || ds.SourceInformation.Title == "") { //Source information does not contain an author errors.Add(Create.Error("Dataset file does not contain a title in the source information. For more information see https://github.com/BHoM/documentation/wiki/IsValidDataset", Create.Location(filePath, Create.LineSpan(1, 1)), documentationLink, TestStatus.Warning, "Dataset source title error" )); result.Status = TestStatus.Warning; } if (ds.SourceInformation.Confidence == Confidence.Undefined) { //Source confidence has not been set errors.Add(Create.Error("Dataset confidence level has not been correctly set. For more information see https://github.com/BHoM/documentation/wiki/IsValidDataset", Create.Location(filePath, Create.LineSpan(1, 1)), documentationLink, TestStatus.Error, "Dataset confidence error" )); result.Status = TestStatus.Error; } if (errors.Count > 0) { result.Information.AddRange(errors.Select(x => x as ITestInformation)); } return(result); }
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); } }
public DatasetSourceFacade(DatasetSource ds) { this.ds = ds; }
public object MapField(BigEndianBinaryReader reader, DatasetSource ds, CarTimestamp ct) { object val = MapData(ReadData(reader)); if (!string.IsNullOrEmpty(dsitem) && ds != null) { IDataItemSource di; if (!ds.TryGetValue(dsitem, out di)) { Type digType = typeof(DataItemSource<>); Type diType = digType.MakeGenericType(outtype); DataItemDescriptor desc = new DataItemDescriptor(dsitem, outtype, null, null, 250); di = (IDataItemSource)Activator.CreateInstance(diType, desc); ds.Add(dsitem, di); } di.Add(val, ct); } return val; }
public object[] MapMessage(object target, DatasetSource ds, CarTimestamp ts, BigEndianBinaryReader reader) { // reader will be positioned at start of payload int nextra = 0; if (methodHasTimestamp) { nextra++; } if (methodHasMsgType) { nextra++; } object[] vals = new object[fields.Count]; object[] param = null; if (method != null) { if (methodIsObjectArray) { param = new object[1 + nextra]; } else { param = new object[fields.Count + nextra]; } if (methodHasTimestamp) { param[0] = ts; if (methodHasMsgType) { param[1] = id; } } else if (methodHasMsgType) { param[0] = id; } } for (int i = 0; i < fields.Count; i++) { object val = fields[i].MapField(reader, ds, ts); if (param != null && !methodIsObjectArray) { param[i + nextra] = val; } vals[i] = val; } if (methodIsObjectArray && param != null) { param[nextra] = vals; } if (method != null) { method.Invoke(target, param); } return(vals); }