public void SendSynchronizedMessage(Messages.Standard.Time synchronized_time)
{
Debug.Log("GPS:Send Sync Messages..."); //+message.header.stamp);
message.header.TimeSynchronization(synchronized_time);
ComposeAndComputeMessage();
Publish(message);
}
public virtual Messages.Standard.Time Now()
{
Messages.Standard.Time stamp = new Messages.Standard.Time();
float time = Time.realtimeSinceStartup;
stamp.secs = (uint)time;
stamp.nsecs = (uint)(1e9 * (time - stamp.secs));
return(stamp);
}
public void SendSynchronizedMessage(Messages.Standard.Time synchronized_time)
{
message.header.TimeSynchronization(synchronized_time);
//Compute current coordinates
message.longitude = rb.velocity.x;
message.latitude = rb.velocity.z;
message.altitude = rb.velocity.y;
Debug.Log("SendSynchronizedMessage velocity=" + rb.velocity.ToString());
Publish(message);
}
private void UpdateMessages()
{
Messages.Standard.Time synced = new Messages.Standard.Time();
float time = Time.realtimeSinceStartup;
synced.secs = (uint)time;
synced.nsecs = (uint)(1e9 * (time - synced.secs));
gps_pub.SendSynchronizedMessage(synced);
gps_speed_pub.SendSynchronizedMessage(synced);
imu_pub.SendSynchronizedMessage(synced);
}
protected override void ReceiveMessage(Messages.Rosgraph.Clock message)
{
this.clock = message.clock;
/*
* position = GetPosition(message).Ros2Unity();
* rotation = GetRotation(message).Ros2Unity();
*/
isMessageReceived = true;
}
public void SendSynchronizedMessage(Messages.Standard.Time synchronized_time)
{
Debug.Log("IMU:Send Sync Messages..." + message.header.stamp); //+message.header.stamp);
message.header.TimeSynchronization(synchronized_time);
message.orientation = GetGeometryQuaternion(rb.rotation.Unity2Ros());
message.orientation_covariance = new float[] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
message.angular_velocity = GetGeometryVector3(rb.angularVelocity.Unity2Ros());
message.angular_velocity_covariance = new float[] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
Vector3 distancePerFrame = rb.position - lastPos;
lastPos = rb.position;
Vector3 speed = distancePerFrame * Time.deltaTime;
message.linear_acceleration = GetGeometryVector3(speed.Unity2Ros());
message.linear_acceleration_covariance = new float[] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
Publish(message);
}
public static void TimeSynchronization(this Messages.Standard.Header header, Messages.Standard.Time sync_time)
{
header.seq++;
header.stamp = sync_time;
}
