void sendGroundtruth()
{
RRSTransform t = new RRSTransform();
t.position = new RRS.Tools.Protobuf.SVector3();
t.orientation = new RRS.Tools.Protobuf.SVector4();
Vector3 convertp = new Vector3();
convertp = Helper.Unity2Ros(transform.position);
t.position.x = convertp.x;
t.position.y = convertp.y;
t.position.z = convertp.z;
var convertr = Helper.Unity2Ros(transform.rotation);
t.orientation.x = convertr.x;
t.orientation.y = convertr.y;
t.orientation.z = convertr.z;
t.orientation.w = convertr.w;
MemoryStream ms = new MemoryStream();
Serializer.Serialize <RRSTransform>(ms, t);
byte[] data = ms.ToArray();
publisher_groundtruth.Send(data);
}
RRSTransform getRRSTransform(Transform t, Links name)
{
RRSTransform rtransform = new RRSTransform();
Quaternion ros_q = Helper.Unity2Ros(t.localRotation);
Vector3 ros_p = Helper.Unity2Ros(t.localPosition);
rtransform.position = new RRS.Tools.Protobuf.SVector3(ros_p.x, ros_p.y, ros_p.z);
rtransform.orientation = new RRS.Tools.Protobuf.SVector4(ros_q.x, ros_q.y, ros_q.z, ros_q.w);
return(rtransform);
}