public void SendTransform(RBS.Messages.geometry_msgs.Transform transform, RBS.Messages.Time time, string frameID, string childFrameID)
{
RBS.Messages.geometry_msgs.TransformStamped transformStamped = new RBS.Messages.geometry_msgs.TransformStamped();
transformStamped.transform = transform;
transformStamped.header.stamp = time;
transformStamped.header.frame_id = frameID;
transformStamped.child_frame_id = childFrameID;
tfMessage.transforms[0] = transformStamped;
transformPublisher.Publish(tfMessage);
}
public RBS.Messages.Time Now()
{
TimeSpan timeSpan = DateTime.Now.ToUniversalTime() - UNIX_EPOCH;
double msecs = timeSpan.TotalMilliseconds;
uint sec = (uint)(msecs / 1000);
RBS.Messages.Time timeMessage = new Messages.Time();
timeMessage.secs = sec;
timeMessage.nsecs = (uint)((msecs / 1000 - sec) * 1e+9);
return(timeMessage);
}
public void SendTransform(UnityEngine.Transform transform, RBS.Messages.Time time, string frameID, string childFrameID)
{
RBS.Messages.geometry_msgs.TransformStamped transformStamped = new RBS.Messages.geometry_msgs.TransformStamped();
RBS.Messages.geometry_msgs.Transform rosTransform = new RBS.Messages.geometry_msgs.Transform();
rosTransform.translation.x = transform.position.x;
rosTransform.translation.y = transform.position.y;
rosTransform.translation.z = transform.position.z;
rosTransform.rotation.w = transform.rotation.w;
rosTransform.rotation.x = transform.rotation.x;
rosTransform.rotation.y = transform.rotation.y;
rosTransform.rotation.z = transform.rotation.z;
transformStamped.transform = rosTransform;
transformStamped.header.stamp = time;
transformStamped.header.frame_id = frameID;
transformStamped.child_frame_id = childFrameID;
tfMessage.transforms[0] = transformStamped;
transformPublisher.Publish(tfMessage);
}
