private void InitializeMessage()
{
clock = new MessageTypes.Rosgraph.Clock();
stamp = new MessageTypes.Std.Time();
// Debug.Log(stamp);
}
public virtual void Now(MessageTypes.Std.Time stamp)
{
uint secs; uint nsecs;
Now(out secs, out nsecs);
stamp.secs = secs; stamp.nsecs = nsecs;
}
public virtual MessageTypes.Std.Time Now()
{
MessageTypes.Std.Time stamp = new MessageTypes.Std.Time();
float time = Time.realtimeSinceStartup;
stamp.secs = (uint)time;
stamp.nsecs = (uint)(1e9 * (time - stamp.secs));
return(stamp);
}
public void SendSynchronizedMessage(MessageTypes.Std.Time synchronized_time)
{
Debug.Log("IMU:Send Sync Messages..." + message.header.stamp); //+message.header.stamp);
/*** to adapt to new ros# new version
* message.header.TimeSynchronization(synchronized_time);
* message.orientation = GetGeometryQuaternion(rb.rotation.Unity2Ros());
* message.orientation_covariance = new double[] {1,0,0,0,1,0,0,0,1};
* message.angular_velocity= GetGeometryVector3(rb.angularVelocity.Unity2Ros());
* message.angular_velocity_covariance = new double[] {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 double[] {1,0,0,0,1,0,0,0,1};
*
* Publish(message); ***/
}
protected override void ReceiveMessage(GoalID message)
{
this.id = message.id;
this.stamp = message.stamp;
parseValues();
}
public virtual void Now(MessageTypes.Std.Time stamp)
{
Now(out stamp.secs, out stamp.nsecs);
}
