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