void FixedUpdate()
{
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin);
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
var data = new GpsData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
IgnoreMapOrigin = IgnoreMapOrigin,
Latitude = location.Latitude,
Longitude = location.Longitude,
Altitude = location.Altitude,
Northing = location.Northing,
Easting = location.Easting,
Orientation = orientation,
};
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, (Action)(() =>
{
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(data);
}
})));
}
}
void FixedUpdate()
{
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin);
var data = new GpsOdometryData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
ChildFrame = ChildFrame,
IgnoreMapOrigin = IgnoreMapOrigin,
Latitude = location.Latitude,
Longitude = location.Longitude,
Altitude = location.Altitude,
Northing = location.Northing,
Easting = location.Easting,
Orientation = transform.rotation,
ForwardSpeed = Vector3.Dot(RigidBody.velocity, transform.forward),
Velocity = RigidBody.velocity,
AngularVelocity = RigidBody.angularVelocity,
WheelAngle = (Dynamics.axles[0].left.steerAngle + Dynamics.axles[0].right.steerAngle) * 0.5f,
};
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, (Action)(() => Writer.Write(data))));
}
}
private void AddReferencePoint(MapOrigin origin)
{
var index = FindObjectsOfType <MapOriginReferencePoint>(true).Length + 1;
var p = new GameObject("ReferencePoint" + index).AddComponent <MapOriginReferencePoint>();
if (SceneView.lastActiveSceneView != null)
{
var camera = SceneView.lastActiveSceneView.camera.transform;
if (Physics.Raycast(new Ray(camera.position, camera.forward), out var hit))
{
p.transform.position = hit.point;
}
}
var gps = origin.GetGpsLocation(p.transform.position);
p.latitue = gps.Latitude;
p.longitude = gps.Longitude;
var mapHolder = FindObjectOfType <MapHolder>().transform;
var holder = mapHolder.Find("ReferencePoints");
if (holder == null)
{
holder = new GameObject("ReferencePoints").transform;
holder.parent = mapHolder;
}
p.transform.parent = holder;
Selection.activeGameObject = p.gameObject;
}
public void Update()
{
if (MapOrigin == null)
{
return;
}
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
float speed = RigidBody.velocity.magnitude;
var gps = MapOrigin.GetGpsLocation(transform.position);
var msg = new CanBusData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
Speed = speed,
Throttle = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0,
Braking = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0,
Steering = Dynamics.SteerInput,
ParkingBrake = Dynamics.HandBrake,
HighBeamSignal = Actions.CurrentHeadLightState == VehicleActions.HeadLightState.HIGH,
LowBeamSignal = Actions.CurrentHeadLightState == VehicleActions.HeadLightState.LOW,
HazardLights = Actions.HazardLights,
FogLights = Actions.FogLights,
LeftTurnSignal = Actions.LeftTurnSignal,
RightTurnSignal = Actions.RightTurnSignal,
Wipers = false,
InReverse = Dynamics.Reverse,
Gear = Mathf.RoundToInt(Dynamics.CurrentGear),
EngineOn = Dynamics.IgnitionStatus == IgnitionStatus.On,
EngineRPM = Dynamics.CurrentRPM,
Latitude = gps.Latitude,
Longitude = gps.Longitude,
Altitude = gps.Altitude,
Orientation = transform.rotation,
Velocity = RigidBody.velocity,
};
if (Bridge != null && Bridge.Status == Status.Connected)
{
Writer.Write(msg, null);
}
}
void FixedUpdate()
{
// Publish Frequency Limit
if (lastUpdateTime.AddSeconds(1.0f / Frequency) > DateTime.Now)
{
return;
}
lastUpdateTime = DateTime.Now;
// Sanity Check
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
// Get Data for Packing
float speed = RigidBody.velocity.magnitude;
var gps = MapOrigin.GetGpsLocation(transform.position);
if (creep_state == true)
{
creep_state = false;
}
else
{
temp_vehicle_speed = speed * 3.60f;
}
// Pack message data
msg = new ChassisData()
{
stamp = DateTime.Now,
frame_id = "",
steering_torque = 0.0f,
engine_rpm = Dynamics.CurrentRPM,
vehicle_speed = temp_vehicle_speed,
throttle_percent = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0,
brake_percent = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0,
steering_angle = -Dynamics.CurrentSteerAngle * Dynamics.SteeringRatio,
brake_status = false,
cruise_start = false,
cruise_cancel = false,
takeover = false,
};
//speed during creep behaviour
if (msg.brake_percent == 0.0f && msg.vehicle_speed >= 7.92f && msg.throttle_percent == 0.0f)
{
creep_state = true;
msg.vehicle_speed = 2.2f * 3.60f;
temp_vehicle_speed = msg.vehicle_speed;
}
// Write Data to Message Queue
Writer.Write(msg, null);
}
public void Init(MapOrigin origin)
{
this.origin = origin;
var gps = origin.GetGpsLocation(origin.transform.position);
latitude = Math.Round(gps.Latitude, 6);
longitude = Math.Round(gps.Longitude, 6);
minSize = new Vector2(250, 120);
maxSize = new Vector2(300, 120);
}
public void RecordJSONLog(Detected3DObject detected3DObject, string labelId, GameObject parent, float cycleTime)
{
var sensorRotation = transform.rotation;
var sensorPosition = transform.position;
var globalPos = parent.transform.position;
var globalRot = parent.transform.rotation;
if (!prevTimes.ContainsKey(labelId))
{
prevTimes.Add(labelId, SimulatorManager.Instance.CurrentTime);
}
// Get GPS coordinates.
bool IgnoreMapOrigin = false;
GpsLocation location = new GpsLocation();
Vector3 position = transform.position;
location = MapOrigin.GetGpsLocation(position, IgnoreMapOrigin);
if (SimulatorManager.Instance.CurrentTime - prevTimes[labelId] > cycleTime)
{
// string labelId = null;
var groundTruth3D = new JSONGroundTruth3D()
{
Id = detected3DObject.Id,
Label = detected3DObject.Label,
Position = (float3)detected3DObject.Position,
Rotation = detected3DObject.Rotation,
Velocity = detected3DObject.LinearVelocity,
Dimension = detected3DObject.Scale,
Time = SimulatorManager.Instance.CurrentTime,
GpsPosition = new Vector3((float)(location.Easting + (IgnoreMapOrigin ? -500000 : 0)),
(float)location.Northing, (float)location.Altitude),
GpsRotation = sensorRotation,
GlobalPosition = globalPos,
GlobalRotation = globalRot
};
if (!LogJsonDetected.ContainsKey(labelId))
{
var jsonGroundTruth3Ds = new List <JSONGroundTruth3D>();
jsonGroundTruth3Ds.Add(groundTruth3D);
LogJsonDetected.Add(labelId, jsonGroundTruth3Ds);
}
else
{
LogJsonDetected[labelId].Add(groundTruth3D);
}
prevTimes[labelId] = SimulatorManager.Instance.CurrentTime;
}
}
void FixedUpdate()
{
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
//Creation of the data file to add the values
/*using (System.IO.StreamWriter sw = System.IO.File.CreateText(@"C:\Users\doria\Desktop\GPS_test.txt"))
* {
* sw.WriteLine("Test");
* }*/
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin);
var orientation = transform.rotation;
var angularVelocity = RigidBody.angularVelocity;
float dLatitude = 0;
float dLongitude = 0;
float dAltitude = 0;
if (activateFailure && time > failTime && time < failStopTime)
{
health = false;
}
else
{
health = true;
}
if (health)
{
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz
if (activateBias)
{
dLatitude = genBias(sLatitude);
dLongitude = genBias(sLongitude);
dAltitude = genBias(sAltitude);
location.Latitude += dLatitude;
location.Longitude += dLongitude;
location.Altitude += dAltitude;
}
}
else
{
location.Latitude = 0;
location.Longitude = 0;
location.Altitude = 0;
orientation.Set(0, 0, 0, 1); // converting to right handed xyz
angularVelocity.Set(0, 0, 0); // converting to right handed xyz
}
data = new GpsOdometryData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
ChildFrame = ChildFrame,
IgnoreMapOrigin = IgnoreMapOrigin,
Latitude = location.Latitude,
Longitude = location.Longitude,
Altitude = location.Altitude,
Northing = location.Northing,
Easting = location.Easting,
Orientation = orientation,
ForwardSpeed = Vector3.Dot(RigidBody.velocity, transform.forward),
Velocity = RigidBody.velocity,
AngularVelocity = angularVelocity,
WheelAngle = Dynamics.WheelAngle,
};
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, (Action)(() => Writer.Write(data))));
}
}
private void Awake()
{
mapOrigin = MapOrigin.Find();
gpsLocation = mapOrigin.GetGpsLocation(Vector3.zero);
}
void WhileInRange(Collider other)
{
GameObject egoGO = transform.parent.gameObject;
GameObject parent = other.transform.parent.gameObject;
if (parent == egoGO)
{
return;
}
if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy)
{
return;
}
if (!Detected.ContainsKey(other))
{
uint id;
string label = null;
float linear_vel;
float angular_vel;
float egoPosY = egoGO.GetComponent <VehicleActions>().Bounds.center.y;
float score = 0.0f;
if (parent.layer == LayerMask.NameToLayer("Agent"))
{
var egoC = parent.GetComponent <VehicleController>();
var egoA = parent.GetComponent <VehicleActions>();
var rb = parent.GetComponent <Rigidbody>();
id = egoC.GTID;
label = "Sedan";
linear_vel = UnityEngine.Vector3.Dot(rb.velocity, parent.transform.forward);
angular_vel = -rb.angularVelocity.y;
}
else if (parent.layer == LayerMask.NameToLayer("NPC"))
{
var npcC = parent.GetComponent <NPCController>();
id = npcC.GTID;
label = npcC.NPCType;
linear_vel = UnityEngine.Vector3.Dot(npcC.GetVelocity(), parent.transform.forward);
angular_vel = -npcC.GetAngularVelocity().y;
}
else if (parent.layer == LayerMask.NameToLayer("Pedestrian"))
{
var pedC = parent.GetComponent <PedestrianController>();
id = pedC.GTID;
label = "Pedestrian";
linear_vel = UnityEngine.Vector3.Dot(pedC.CurrentVelocity, parent.transform.forward);
angular_vel = -pedC.CurrentAngularVelocity.y;
}
else
{
return;
}
UnityEngine.Vector3 size = ((BoxCollider)other).size;
// Convert from (Right/Up/Forward) to (Forward/Left/Up)
size.Set(size.z, size.x, size.y);
if (size.magnitude == 0)
{
return;
}
//Bounding Box Orientation
//var Rot = Quaternion.Euler(parent.transform.rotation.eulerAngles);
var rot_euler = parent.transform.rotation.eulerAngles;
var Rot = UnityEngine.Quaternion.Euler(rot_euler);
//Bounding Box Position
if (MapOrigin == null)
{
return;
}
UnityEngine.Vector3 bounding_box_center = parent.transform.TransformPoint(((BoxCollider)other).center);
var location = MapOrigin.GetGpsLocation(bounding_box_center /*parent.transform.position*/);
if (!string.IsNullOrEmpty(label))
{
score = 1.0f;
}
Detected.Add(other, new Detected3DObjectPerc()
{
Id = id,
Label = label,
Score = score,
easting = location.Easting,
northing = location.Northing,
Rotation = Rot,
Scale = size,
LinearVelocity = new UnityEngine.Vector3(linear_vel, 0, 0), // Linear velocity in forward direction of objects, in meters/sec
AngularVelocity = new UnityEngine.Vector3(0, 0, angular_vel), // Angular velocity around up axis of objects, in radians/sec
});
}
}
void WhileInRange(Collider other)
{
GameObject egoGO = transform.parent.gameObject;
GameObject parent = other.transform.parent.gameObject;
if (parent == egoGO)
{
return;
}
if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy)
{
return;
}
if (!Detected.ContainsKey(other))
{
uint id;
string label;
float linear_vel;
float angular_vel;
if (parent.layer == LayerMask.NameToLayer("NPC"))
{
var npcC = parent.GetComponent <NPCController>();
id = npcC.GTID;
label = npcC.NPCType;
linear_vel = UnityEngine.Vector3.Dot(npcC.GetVelocity(), parent.transform.forward);
angular_vel = -npcC.GetAngularVelocity().y;
}
else
{
return;
}
//Bounding Box Size
UnityEngine.Vector3 size = ((BoxCollider)other).size;
if (size.magnitude == 0)
{
return;
}
// Convert from (Right/Up/Forward) to (Forward/Left/Up)
size.Set(size.z, size.x, size.y);
//Bounding Box Orientation
//var Rot = Quaternion.Euler(parent.transform.rotation.eulerAngles);
var rot_euler = parent.transform.rotation.eulerAngles;
//90 degree phase shift to match with localization
rot_euler.y = 90 - rot_euler.y;
var Rot = UnityEngine.Quaternion.Euler(rot_euler);
//Bounding Box Position
if (MapOrigin == null)
{
return;
}
UnityEngine.Vector3 bounding_box_center = parent.transform.TransformPoint(((BoxCollider)other).center);
var location = MapOrigin.GetGpsLocation(bounding_box_center /*parent.transform.position*/);
Detected.Add(other, new Detected3DObjectAdhoc()
{
Id = id,
Label = label,
Score = 1.0f,
easting = location.Easting,
northing = location.Northing,
Rotation = Rot,
Scale = size,
LinearVelocity = new UnityEngine.Vector3(linear_vel, 0, 0), // Linear velocity in forward direction of objects, in meters/sec
AngularVelocity = new UnityEngine.Vector3(0, 0, angular_vel), // Angular velocity around up axis of objects, in radians/sec
});
}
}
void FixedUpdate()
{
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin);
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
var angularVelocity = RigidBody.angularVelocity;
angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz
var data = new GpsOdometryData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = SendSequence++,
ChildFrame = ChildFrame,
IgnoreMapOrigin = IgnoreMapOrigin,
Latitude = location.Latitude,
Longitude = location.Longitude,
Altitude = location.Altitude,
Northing = location.Northing,
Easting = location.Easting,
Orientation = orientation,
ForwardSpeed = Vector3.Dot(RigidBody.velocity, transform.forward),
Velocity = RigidBody.velocity,
AngularVelocity = angularVelocity,
WheelAngle = Dynamics.WheelAngle,
};
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, (Action)(() =>
{
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(data);
}
})));
}
}
void WhileInRange(Collider other)
{
GameObject egoGO = transform.parent.gameObject;
GameObject parent = other.transform.parent.gameObject;
if (parent == egoGO)
{
return;
}
if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy)
{
return;
}
uint id;
string label;
Vector3 velocity;
float angular_speed; // Angular speed around up axis of objects, in radians/sec
if (parent.layer == LayerMask.NameToLayer("Agent"))
{
var egoC = parent.GetComponent <VehicleController>();
var rb = parent.GetComponent <Rigidbody>();
id = egoC.GTID;
label = "Sedan";
velocity = rb.velocity;
angular_speed = rb.angularVelocity.y;
}
else if (parent.layer == LayerMask.NameToLayer("NPC"))
{
var npcC = parent.GetComponent <NPCController>();
id = npcC.GTID;
label = npcC.NPCLabel;
velocity = npcC.GetVelocity();
angular_speed = npcC.GetAngularVelocity().y;
}
else if (parent.layer == LayerMask.NameToLayer("Pedestrian"))
{
var pedC = parent.GetComponent <PedestrianController>();
id = pedC.GTID;
label = "Pedestrian";
velocity = pedC.CurrentVelocity;
angular_speed = pedC.CurrentAngularVelocity.y;
}
else
{
return;
}
Vector3 size = ((BoxCollider)other).size;
if (size.magnitude == 0)
{
return;
}
// Linear speed in forward direction of objects, in meters/sec
float speed = Vector3.Dot(velocity, parent.transform.forward);
// Local position of object in ego local space
Vector3 relPos = transform.InverseTransformPoint(parent.transform.position);
// Relative rotation of objects wrt ego frame
Quaternion relRot = Quaternion.Inverse(transform.rotation) * parent.transform.rotation;
var mapRotation = MapOrigin.transform.localRotation;
velocity = Quaternion.Inverse(mapRotation) * velocity;
var heading = parent.transform.localEulerAngles.y - mapRotation.eulerAngles.y;
// Center of bounding box
GpsLocation location = MapOrigin.GetGpsLocation(((BoxCollider)other).bounds.center);
GpsData gps = new GpsData()
{
Easting = location.Easting,
Northing = location.Northing,
Altitude = location.Altitude,
};
if (!Detected.ContainsKey(id))
{
var det = new Detected3DObject()
{
Id = id,
Label = label,
Score = 1.0f,
Position = relPos,
Rotation = relRot,
Scale = size,
LinearVelocity = new Vector3(speed, 0, 0),
AngularVelocity = new Vector3(0, 0, angular_speed),
Velocity = velocity,
Gps = gps,
Heading = heading,
TrackingTime = 0f,
};
Detected.Add(id, new Tuple <Detected3DObject, Collider>(det, other));
}
else
{
var det = Detected[id].Item1;
det.Position = relPos;
det.Rotation = relRot;
det.LinearVelocity = new Vector3(speed, 0, 0);
det.AngularVelocity = new Vector3(0, 0, angular_speed);
det.Acceleration = (velocity - det.Velocity) / Time.fixedDeltaTime;
det.Velocity = velocity;
det.Gps = gps;
det.Heading = heading;
det.TrackingTime += Time.fixedDeltaTime;
}
CurrentIDs.Add(id);
}
void FixedUpdate()
{
// Publish Frequency Limit
if (lastUpdateTime.AddSeconds(1.0f / Frequency) > DateTime.Now)
{
return;
}
lastUpdateTime = DateTime.Now;
// Sanity Check
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
// Get Data for Packing
var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin);
// INSPVA
WriterInsPVA.Write(new novatelInsPvaData()
{
stamp = DateTime.Now,
frame_id = FrameId,
week = 0,
seconds = 0,
latitude = location.Latitude,
longitude = location.Longitude,
height = location.Altitude,
north_velocity = RigidBody.velocity.z,
east_velocity = RigidBody.velocity.x,
up_velocity = RigidBody.velocity.y,
roll = transform.rotation.eulerAngles.x,
pitch = transform.rotation.eulerAngles.z,
azimuth = transform.rotation.eulerAngles.y,
status = 3
});
// BESTPOS
WriterBestPos.Write(new novatelBestPosData()
{
stamp = DateTime.Now,
frame_id = FrameId,
lat = location.Latitude,
lon = location.Longitude,
hgt = location.Altitude,
sol_status = 0,
sol_status_str = "SOL_COMPUTED",
pos_type = 50,
pos_type_str = "NARROW_INT",
});
// RAWIMUX
Vector3 deltaV = prev_velocity - RigidBody.velocity;
prev_velocity = RigidBody.velocity;
// Gyro
Vector3 deltaA = RigidBody.angularVelocity;
// IMU Status
int imuStatus = 0;
// Status Sequence Counter
rawSeqNum++;
// Device-Specific values
switch (ImuType)
{
case ImuTypeEnum.IMU_KVH_1750:
// LSB Scale Factors
deltaV /= (float)(0.1 / (3600.0 * 256.0));
deltaA /= (float)(0.05 / Math.Pow(2, 15));
// Gyro & Accel Valid Status
imuStatus |= 0x00000077;
imuStatus |= (rawSeqNum << 8);
imuStatus |= (ImuCelsius << 16);
break;
default:
Debug.Log("Unknown Novatel IMU type: " + ImuType);
break;
}
WriterRawImuX.Write(new novatelRawImuXData()
{
stamp = DateTime.Now,
frame_id = FrameId,
error = 0,
type = (byte)ImuType,
week = 0,
seconds = 0,
status = imuStatus,
z_accel = (int)deltaV.y,
y_accel_neg = -(int)deltaV.z,
x_accel = (int)deltaV.x,
z_gyro = -(int)deltaA.y,
y_gyro_neg = -(int)deltaA.z,
x_gyro = (int)deltaA.x,
});
WriterCorrIMU.Write(new novatelCorrIMUData()
{
stamp = DateTime.Now,
frame_id = FrameId,
week = 0,
seconds = 0,
pitch_rate = (int)deltaA.x,
roll_rate = (int)deltaA.z,
yaw_rate = -(int)deltaA.y,
lateral_acc = (int)deltaV.x,
longitudinal_acc = (int)deltaV.z,
vertical_acc = (int)deltaV.y,
});
WriterHeading.Write(new novatelHeadingData()
{
stamp = DateTime.Now,
frame_id = FrameId,
sol_status = 0,
sol_status_str = "0",
pos_type = 0,
pos_type_str = "0",
length = 0,
heading = 0,
pitch = 0,
heading_std_dev = 0,
pitch_std_dev = 0,
station_id = "0",
num_svs = 0,
num_sol_in_svs = 0,
num_obs = 0,
num_multi = 0,
sol_source = 0,
ext_sol_stat = 0,
galileo_beidou_sig_mask = 0,
gps_glonass_sig_mask = 0,
});
// MARK3COUNT
if (DateTime.Now > lastUpdateTimeMarkCount)
{
// Publish Frequency Limit (1Hz)
lastUpdateTimeMarkCount = DateTime.Now.AddSeconds(1.0f);
// Get Wheel Collider
WheelCollider RR = Dynamics.axles[1].right;
// Publish Message
WriterMarkCount.Write(new novatelMarkCountData()
{
stamp = DateTime.Now,
frame_id = FrameId,
mark_num = 3,
period = 1000000,
count = (ushort)Math.Round(RR.rpm * EncoderRPMtoPPS),
});
}
}
