public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage || !IsEnabled)
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
Vector3 angularVelocity = mainRigidbody.angularVelocity;
System.DateTime GPSepoch = new System.DateTime(1980, 1, 6, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - GPSepoch).TotalSeconds + 18.0f;
float measurement_span = (float)Time.fixedDeltaTime;
// Debug.Log(measurement_time + ", " + measurement_span);
// Debug.Log("Linear Acceleration: " + linear_acceleration.x.ToString("F1") + ", " + linear_acceleration.y.ToString("F1") + ", " + linear_acceleration.z.ToString("F1"));
// Debug.Log("Angular Velocity: " + angular_velocity.x.ToString("F1") + ", " + angular_velocity.y.ToString("F1") + ", " + angular_velocity.z.ToString("F1"));
var angles = Target.transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = -angles.y;
Quaternion orientation_unity = Quaternion.Euler(roll, pitch, yaw);
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
// for odometry frame position
odomPosition.x += currVelocity.z * Time.fixedDeltaTime * Mathf.Cos(yaw * (Mathf.PI / 180.0f));
odomPosition.y += currVelocity.z * Time.fixedDeltaTime * Mathf.Sin(yaw * (Mathf.PI / 180.0f));
acceleration += transform.InverseTransformDirection(Physics.gravity);
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -acceleration.y,
};
var angular_velocity = new Ros.Point3D()
{
x = -angularVelocity.z,
y = angularVelocity.x,
z = -angularVelocity.y,
};
ApolloWriterImu.Publish(new Ros.Apollo.Imu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
});
var apolloIMUMessage = new Ros.CorrectedImu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
imu = new Ros.ApolloPose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_acceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
angular_velocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
euler_angles = new Ros.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad
}
}
};
ApolloWriterCorrectedImu.Publish(apolloIMUMessage);
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
var linear_acceleration = new Apollo.Common.Point3D()
{
X = acceleration.x,
Y = acceleration.z,
Z = -acceleration.y,
};
var angular_velocity = new Apollo.Common.Point3D()
{
X = -angularVelocity.z,
Y = angularVelocity.x,
Z = -angularVelocity.y,
};
Apollo35WriterImu.Publish(new Apollo.Drivers.Gnss.Imu()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time
},
MeasurementTime = measurement_time,
MeasurementSpan = measurement_span,
LinearAcceleration = linear_acceleration,
AngularVelocity = angular_velocity
});
var apolloIMUMessage = new Apollo.Localization.CorrectedImu()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time
},
Imu = new Apollo.Localization.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
LinearAcceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
AngularVelocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
Heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
EulerAngles = new Apollo.Common.Point3D()
{
X = roll * Mathf.Deg2Rad,
Y = pitch * Mathf.Deg2Rad,
Z = yaw * Mathf.Deg2Rad,
}
}
};
Apollo35WriterCorrectedImu.Publish(apolloIMUMessage);
}
else if (TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1 || TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
var imu_msg = new Ros.Imu()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = ImuFrameId,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
orientation_covariance = new double[9],
angular_velocity = new Ros.Vector3()
{
x = angularVelocity.z,
y = -angularVelocity.x,
z = angularVelocity.y,
},
angular_velocity_covariance = new double[9],
linear_acceleration = new Ros.Vector3()
{
x = acceleration.z,
y = -acceleration.x,
z = acceleration.y,
},
linear_acceleration_covariance = new double[9],
};
AutowareWriterImu.Publish(imu_msg);
var odom_msg = new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence,
frame_id = OdometryFrameId,
},
child_frame_id = OdometryChildFrameId,
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = odomPosition.x,
y = odomPosition.y,
z = odomPosition.z,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
},
covariance = new double[36],
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3(currVelocity.z, -currVelocity.x, currVelocity.y),
angular = new Ros.Vector3(angularVelocity.z, -angularVelocity.x, angularVelocity.y),
},
covariance = new double[36],
},
};
AutowareWriterOdometry.Publish(odom_msg);
}
}
public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage || !IsEnabled)
{
return;
}
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
CurrTimestamp = System.DateTime.UtcNow - Unixepoch;
IsFirstFixedUpdate = false;
}
else
{
CurrTimestamp = CurrTimestamp.Add(Interval);
}
if (TimeSpan.Compare(CurrTimestamp, LastTimestamp) == -1) // if CurrTimestamp < LastTimestamp
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
Vector3 angularVelocity = mainRigidbody.angularVelocity;
var angles = Target.transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = -angles.y;
Quaternion orientation_unity = Quaternion.Euler(roll, pitch, yaw);
double measurement_time = (double)CurrTimestamp.TotalSeconds;
float measurement_span = (float)(1 / Frequency);
// for odometry frame position
odomPosition.x += currVelocity.z * Time.fixedDeltaTime * Mathf.Cos(yaw * (Mathf.PI / 180.0f));
odomPosition.y += currVelocity.z * Time.fixedDeltaTime * Mathf.Sin(yaw * (Mathf.PI / 180.0f));
acceleration += transform.InverseTransformDirection(Physics.gravity);
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -acceleration.y,
};
var angular_velocity = new Ros.Point3D()
{
x = -angularVelocity.z,
y = angularVelocity.x,
z = -angularVelocity.y,
};
var apolloImuMsg = new Ros.Apollo.Imu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
};
var apolloCorrectedImuMsg = new Ros.CorrectedImu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
imu = new Ros.ApolloPose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_acceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
angular_velocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
euler_angles = new Ros.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad
}
}
};
lock (MessageQueue)
{
MessageQueue.Enqueue(new Tuple <TimeSpan, Action>(CurrTimestamp, () => {
ApolloWriterImu.Publish(apolloImuMsg);
ApolloWriterCorrectedImu.Publish(apolloCorrectedImuMsg);
}));
}
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
var linear_acceleration = new apollo.common.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -acceleration.y,
};
var angular_velocity = new apollo.common.Point3D()
{
x = -angularVelocity.z,
y = angularVelocity.x,
z = -angularVelocity.y,
};
var apollo35ImuMsg = new apollo.drivers.gnss.Imu()
{
header = new apollo.common.Header()
{
timestamp_sec = measurement_time
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
};
var apollo35CorrectedImuMsg = new apollo.localization.CorrectedImu()
{
header = new apollo.common.Header()
{
timestamp_sec = measurement_time
},
imu = new apollo.localization.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_acceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
angular_velocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
euler_angles = new apollo.common.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad,
}
}
};
lock (MessageQueue)
{
MessageQueue.Enqueue(new Tuple <TimeSpan, Action>(CurrTimestamp, () => {
Apollo35WriterImu.Publish(apollo35ImuMsg);
Apollo35WriterCorrectedImu.Publish(apollo35CorrectedImuMsg);
}));
}
}
else if (TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1 || TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
var nanoSec = 1000000 * CurrTimestamp.TotalMilliseconds;
var autowareImuMsg = new Ros.Imu()
{
header = new Ros.Header()
{
stamp = new Ros.Time()
{
secs = (long)nanoSec / 1000000000,
nsecs = (uint)nanoSec % 1000000000,
},
seq = Sequence++,
frame_id = ImuFrameId,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
orientation_covariance = new double[9],
angular_velocity = new Ros.Vector3()
{
x = angularVelocity.z,
y = -angularVelocity.x,
z = angularVelocity.y,
},
angular_velocity_covariance = new double[9],
linear_acceleration = new Ros.Vector3()
{
x = acceleration.z,
y = -acceleration.x,
z = acceleration.y,
},
linear_acceleration_covariance = new double[9],
};
var autowareOdomMsg = new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = new Ros.Time()
{
secs = (long)nanoSec / 1000000000,
nsecs = (uint)nanoSec % 1000000000,
},
seq = Sequence,
frame_id = OdometryFrameId,
},
child_frame_id = OdometryChildFrameId,
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = odomPosition.x,
y = odomPosition.y,
z = odomPosition.z,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
},
covariance = new double[36],
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3(currVelocity.z, -currVelocity.x, currVelocity.y),
angular = new Ros.Vector3(angularVelocity.z, -angularVelocity.x, angularVelocity.y),
},
covariance = new double[36],
},
};
lock (MessageQueue)
{
MessageQueue.Enqueue(new Tuple <TimeSpan, Action>(CurrTimestamp, () => {
AutowareWriterImu.Publish(autowareImuMsg);
if (TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
AutowareWriterOdometry.Publish(autowareOdomMsg);
}
}));
}
}
}