public static Imu ConvertFrom(ImuData data)
{
return(new Imu()
{
header = new Header()
{
stamp = ConvertTime(data.Time),
seq = data.Sequence,
frame_id = data.Frame,
},
orientation = Convert(data.Orientation),
orientation_covariance = new double[9],
angular_velocity = new Vector3()
{
x = data.AngularVelocity.z, y = -data.AngularVelocity.x, z = data.AngularVelocity.y
},
angular_velocity_covariance = new double[9],
linear_acceleration = new Vector3()
{
x = data.Acceleration.z, y = -data.Acceleration.x, z = data.Acceleration.y
},
linear_acceleration_covariance = new double[9],
});
}
public static Ros.Imu ConvertFrom(ImuData data)
{
return(new Ros.Imu()
{
header = new Ros.Header()
{
stamp = ConvertTime(data.Time),
seq = data.Sequence,
frame_id = data.Frame,
},
orientation = Convert(data.Orientation),
orientation_covariance = new double[9] {
0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001
},
angular_velocity = ConvertToVector(data.AngularVelocity),
angular_velocity_covariance = new double[9] {
0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001
},
linear_acceleration = ConvertToVector(data.Acceleration),
linear_acceleration_covariance = new double[9] {
0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001
},
});
}
private async UniTask ThreadMethod()
{
IPEndPoint remoteEp = null;
Header header = default;
ImuData imu = default;
ButtonData button = default;
var hasError = false;
await UniTask.SwitchToThreadPool();
while (!cancel.IsCancellationRequested)
{
hasError = false;
try
{
var data = udp.Receive(ref remoteEp);
header = new Header(data.Take(HeaderDef.HeaderLength).ToArray());
var body = data.Skip(HeaderDef.HeaderLength).ToArray();
if (header.DataId == HeaderDef.ImuDataId)
{
if (TryReadImuData(body, ref imu))
{
AhrsData = imu.Quaternion;
}
}
else if (header.DataId == HeaderDef.ButtonDataId)
{
if (TryReadButtonData(body, ref button))
{
if (button.ButtonA == ButtonState.Push)
{
Debug.Log("ButtonA Clicked");
IsButtonAClicked = true;
}
if (button.ButtonB == ButtonState.Push)
{
Debug.Log("ButtonB Clicked");
}
}
}
}
catch (Exception e)
{
Debug.Log(e);
hasError = true;
}
if (hasError)
{
await UniTask.Delay(TimeSpan.FromSeconds(3), cancellationToken : cancel.Token);
}
else
{
await UniTask.Delay(20, cancellationToken : cancel.Token);
}
}
}
private bool TryReadImuData(byte[] bytes, ref ImuData imu)
{
var timestamp = bytes.ToUInt(0);
var acc = bytes.ToVector3(4);
var gyro = bytes.ToVector3(16);
var quat = bytes.ToQuaternion(28);
imu = new ImuData(timestamp, acc, gyro, quat);
return(true);
}
public static apollo.drivers.gnss.Imu ConvertFrom(ImuData data)
{
return(new apollo.drivers.gnss.Imu()
{
header = new apollo.common.Header()
{
timestamp_sec = data.Time,
sequence_num = data.Sequence,
},
measurement_time = GpsUtils.UtcSecondsToGpsSeconds(data.Time),
measurement_span = (float)data.MeasurementSpan,
linear_acceleration = ConvertToPoint(new Vector3(data.Acceleration.x, data.Acceleration.y, data.Acceleration.z)),
angular_velocity = ConvertToPoint(new Vector3(data.AngularVelocity.x, data.AngularVelocity.y, data.AngularVelocity.z)),
});
}
private IEnumerator ThreadMethod()
{
IPEndPoint remoteEp = null;
Header header = default;
ImuData imu = default;
ButtonData button = default;
while (true)
{
try
{
var data = udp.Receive(ref remoteEp);
header = new Header(data.Take(HeaderDef.HeaderLength).ToArray());
var body = data.Skip(HeaderDef.HeaderLength).ToArray();
if (header.DataId == HeaderDef.ImuDataId)
{
if (TryReadImuData(body, ref imu))
{
AhrsData = imu.Quaternion;
}
}
else if (header.DataId == HeaderDef.ButtonDataId)
{
if (TryReadButtonData(body, ref button))
{
if (button.ButtonA == ButtonState.Push)
{
Debug.Log("ButtonA Clicked");
IsButtonAClicked = true;
}
if (button.ButtonB == ButtonState.Push)
{
Debug.Log("ButtonB Clicked");
}
}
}
}
catch (Exception e)
{
Debug.Log(e);
}
// 1ms is less than Ryap(10ms) and a little wait for unlock main thread
yield return(new WaitForSeconds(0.001f));
}
}
public static Apollo.Imu ApolloConvertFrom(ImuData data)
{
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
return(new Apollo.Imu()
{
header = new Apollo.Header()
{
timestamp_sec = data.Time,
sequence_num = data.Sequence,
},
measurement_time = data.Time,
measurement_span = (float)data.MeasurementSpan,
linear_acceleration = new Apollo.Point3D()
{
x = data.Acceleration.x, y = data.Acceleration.z, z = -data.Acceleration.y
},
angular_velocity = new Apollo.Point3D()
{
x = -data.AngularVelocity.z, y = data.AngularVelocity.x, z = -data.AngularVelocity.y
},
});
}
void FixedUpdate()
{
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var velocity = transform.InverseTransformDirection(RigidBody.velocity);
var acceleration = (velocity - LastVelocity) / Time.fixedDeltaTime;
LastVelocity = velocity;
acceleration -= transform.InverseTransformDirection(Physics.gravity);
var angularVelocity = RigidBody.angularVelocity;
var angles = transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = -angles.y;
var orientation = Quaternion.Euler(roll, pitch, yaw);
data = new ImuData()
{
Name = Name,
Frame = Frame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = transform.position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
var correctedData = new CorrectedImuData()
{
Name = Name,
Frame = CorrectedFrame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = transform.position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
if (Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, Time.fixedDeltaTime, (Action)(() => {
Writer.Write(data);
if (CorrectedWriter != null)
{
CorrectedWriter.Write(correctedData);
}
})));
}
Sequence++;
}
void FixedUpdate()
{
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var position = transform.position;
position.Set(position.z, -position.x, position.y);
minX = Mathf.Min(minX, position.x);
maxX = Mathf.Max(maxX, position.x);
minY = Mathf.Min(minY, position.y);
maxY = Mathf.Max(maxY, position.y);
minZ = Mathf.Min(minZ, position.z);
maxZ = Mathf.Max(maxZ, position.z);
var velocity = transform.InverseTransformDirection(Dynamics.Velocity);
velocity.Set(velocity.z, -velocity.x, velocity.y);
var acceleration = (velocity - LastVelocity) / Time.fixedDeltaTime;
LastVelocity = velocity;
var localGravity = transform.InverseTransformDirection(Physics.gravity);
acceleration -= new Vector3(localGravity.z, -localGravity.x, localGravity.y);
var angularVelocity = Dynamics.AngularVelocity;
angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
minGyroX = Mathf.Min(minGyroX, orientation.x);
maxGyroX = Mathf.Max(maxGyroX, orientation.x);
minGyroY = Mathf.Min(minGyroY, orientation.y);
maxGyroY = Mathf.Max(maxGyroY, orientation.y);
minGyroZ = Mathf.Min(minGyroZ, orientation.z);
maxGyroZ = Mathf.Max(maxGyroZ, orientation.z);
var data = new ImuData()
{
Name = Name,
Frame = Frame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
latestData = data;
var correctedData = new CorrectedImuData()
{
Name = Name,
Frame = CorrectedFrame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
if (Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, Time.fixedDeltaTime, (Action)(() =>
{
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(data);
CorrectedWriter?.Invoke(correctedData);
}
})));
}
Sequence++;
}
void FixedUpdate()
{
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var position = transform.position;
position.Set(position.z, -position.x, position.y);
var velocity = transform.InverseTransformDirection(RigidBody.velocity);
velocity.Set(velocity.z, -velocity.x, velocity.y);
var acceleration = (velocity - LastVelocity) / Time.fixedDeltaTime;
LastVelocity = velocity;
var localGravity = transform.InverseTransformDirection(Physics.gravity);
acceleration -= new Vector3(localGravity.z, -localGravity.x, localGravity.y);
var angularVelocity = RigidBody.angularVelocity;
angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
data = new ImuData()
{
Name = Name,
Frame = Frame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
var correctedData = new CorrectedImuData()
{
Name = Name,
Frame = CorrectedFrame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
if (Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, Time.fixedDeltaTime, (Action)(() => {
Writer.Write(data);
if (CorrectedWriter != null)
{
CorrectedWriter.Write(correctedData);
}
})));
}
Sequence++;
}
void FixedUpdate()
{
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
IsFirstFixedUpdate = false;
}
var time = SimulatorManager.Instance.CurrentTime;
if (time < LastTimestamp)
{
return;
}
var position = transform.position;
position.Set(position.z, -position.x, position.y);
var velocity = transform.InverseTransformDirection(RigidBody.velocity);
velocity.Set(velocity.z, -velocity.x, velocity.y);
var acceleration = (velocity - LastVelocity) / Time.fixedDeltaTime;
var localGravity = transform.InverseTransformDirection(Physics.gravity);
var angularVelocity = RigidBody.angularVelocity;
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w);
if (activateFailure && time > failTime && time < failStopTime)
{
health = false;
healthWasFalse = true;
}
else
{
health = true;
}
if (!initialized)
{
LastPosition = position;
LastVelocity = velocity;
LastOrientation = orientation;
initialized = true;
}
else
{
if (health)
{
if (healthWasFalse)
{
position = LastPosition + velocity * Time.fixedDeltaTime;
}
LastVelocity = velocity;
LastPosition = position;
LastOrientation = orientation;
acceleration -= new Vector3(localGravity.z, -localGravity.x, localGravity.y);
angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y);
if (activateBias)
{
dAcceleration = new Vector3(genBias(sAcceleration), genBias(sAcceleration), genBias(sAcceleration));
acceleration += dAcceleration;
dVelocity = dVelocity + dAcceleration * Time.fixedDeltaTime;
velocity += dVelocity;
dPosition = dPosition + dVelocity * Time.fixedDeltaTime;
position += dPosition;
dAngularVelocity = new Vector3(genBias(sAngularVelocity), genBias(sAngularVelocity), genBias(sAngularVelocity));
angularVelocity += dAngularVelocity;
dAttitude = dAttitude + dAngularVelocity * Time.fixedDeltaTime;
var attitude = dAttitude + orientation.eulerAngles;
orientation = Quaternion.Euler(attitude.x, attitude.y, attitude.z);
}
}
else
{
position = LastPosition;
velocity = LastVelocity;
orientation = LastOrientation;
acceleration = new Vector3(0, 0, 0);
angularVelocity.Set(0, 0, 0);
}
}
/*position.Set(position.z, -position.x, position.y);
* velocity.Set(velocity.z, -velocity.x, velocity.y);
* LastVelocity = velocity;
* acceleration -= new Vector3(localGravity.z, -localGravity.x, localGravity.y);
* angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz
* orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz*/
data = new ImuData()
{
Name = Name,
Frame = Frame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
var correctedData = new CorrectedImuData()
{
Name = Name,
Frame = CorrectedFrame,
Time = time,
Sequence = Sequence,
MeasurementSpan = Time.fixedDeltaTime,
Position = position,
Orientation = orientation,
Acceleration = acceleration,
LinearVelocity = velocity,
AngularVelocity = angularVelocity,
};
if (Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
lock (MessageQueue)
{
MessageQueue.Enqueue(Tuple.Create(time, Time.fixedDeltaTime, (Action)(() => {
Writer.Write(data);
if (CorrectedWriter != null)
{
CorrectedWriter.Write(correctedData);
}
})));
}
Sequence++;
}
