public static Apollo.CorrectedImu ApolloConvertFrom(CorrectedImuData data)
{
var angles = data.Orientation.eulerAngles;
float roll = angles.x;
float pitch = angles.y;
float yaw = angles.z;
return(new Apollo.CorrectedImu()
{
header = new Apollo.Header()
{
timestamp_sec = data.Time,
},
imu = new Apollo.Pose()
{
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
},
heading = yaw,
euler_angles = new Apollo.Point3D()
{
x = roll * UnityEngine.Mathf.Deg2Rad,
y = pitch * UnityEngine.Mathf.Deg2Rad,
z = yaw * UnityEngine.Mathf.Deg2Rad,
}
}
});
}
public static apollo.localization.CorrectedImu ConvertFrom(CorrectedImuData data)
{
var orientation = ConvertToRfu(data.Orientation);
var angles = orientation.eulerAngles;
float roll = angles.x;
float pitch = angles.y;
float yaw = angles.z;
return(new apollo.localization.CorrectedImu()
{
header = new apollo.common.Header()
{
timestamp_sec = data.Time,
},
imu = new apollo.localization.Pose()
{
linear_acceleration = ConvertToPoint(new Vector3(data.GlobalAcceleration.x, data.GlobalAcceleration.y, data.GlobalAcceleration.z)),
angular_velocity = ConvertToPoint(new Vector3(data.GlobalAngularVelocity.x, data.GlobalAngularVelocity.y, data.GlobalAngularVelocity.z)),
heading = yaw,
euler_angles = new apollo.common.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad,
}
}
});
}
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++;
}
