public override byte[] Serialize(bool partofsomethingelse)
{
int currentIndex = 0, length = 0;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
List <byte[]> pieces = new List <byte[]>();
GCHandle h;
//transform
if (transform == null)
{
transform = new TransformStamped();
}
pieces.Add(transform.Serialize(true));
//error
if (error == null)
{
error = new Messages.tf2_msgs.TF2Error();
}
pieces.Add(error.Serialize(true));
//combine every array in pieces into one array and return it
int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
int __a_b__e = 0;
byte[] __a_b__d = new byte[__a_b__f];
foreach (var __p__ in pieces)
{
Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
__a_b__e += __p__.Length;
}
return(__a_b__d);
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//transforms
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (transforms == null)
{
transforms = new TransformStamped[arraylength];
}
else
{
Array.Resize(ref transforms, arraylength);
}
for (int i = 0; i < transforms.Length; i++)
{
//transforms[i]
transforms[i] = new TransformStamped(SERIALIZEDSTUFF, ref currentIndex);
}
}
private void PubTF()
{
// System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
// sw.Start();
this.isPublishing = true;
TransformStamped[] transformStampedArray = new TransformStamped[localTfInfoList.Count];
// Add local TF infos
for (int i = 0; i < localTfInfoList.Count; i++)
{
//localTfInfoList[i].UpdateTransformForLocal();
localTfInfoList[i].UpdateTransformForGlobal();
localTfInfoList[i].transformStamped.header.Update();
transformStampedArray[i] = localTfInfoList[i].transformStamped;
}
this.transformStampedMsg.msg = transformStampedArray;
Thread thread = new Thread(new ThreadStart(SendTF));
thread.Start();
// sw.Stop();
// Debug.Log("tf sending time="+sw.Elapsed);
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = PR2Common.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
if (localLink.name == PR2Common.Link.base_footprint.ToString())
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = PR2Common.OdomName;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <PR2PubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
public void TestTransformStampedMessage()
{
Assert.True(BuiltIns.GetMessageType <MyTransformStamped>() == MyTransformStamped.RosMessageType);
Assert.True(BuiltIns.GetMd5Sum <MyTransformStamped>() == BuiltIns.GetMd5Sum <TransformStamped>());
time now = time.Now();
TransformStamped real = new TransformStamped()
{
Header = new Header(1, now, "Frame"),
ChildFrameId = "Abcd",
Transform = Transform.Identity
};
MyTransformStamped wrapped = new MyTransformStamped()
{
Header = new Header(1, now, "Frame"),
ChildFrameId = "Abcd",
Transform = Transform.Identity
};
byte[] messageBytes = real.SerializeToArray();
byte[] otherMessageBytes = wrapped.SerializeToArray();
Assert.True(messageBytes.SequenceEqual(otherMessageBytes));
var otherWrapped = wrapped.DeserializeFromArray(messageBytes);
Assert.True(wrapped.Header == real.Header);
Assert.True(wrapped.ChildFrameId == real.ChildFrameId);
Assert.True(wrapped.Transform == real.Transform);
}
private IEnumerator PubTF()
{
yield return(new WaitForEndOfFrame());
// System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
// sw.Start();
TransformStamped[] transformStampedArray = new TransformStamped[localTfInfoList.Count];
// Add local TF infos
for (int i = 0; i < localTfInfoList.Count; i++)
{
localTfInfoList[i].UpdateTransformForLocal();
localTfInfoList[i].transformStamped.header.Update();
transformStampedArray[i] = localTfInfoList[i].transformStamped;
}
this.transformStampedMsg.msg = transformStampedArray;
Task.Run(() =>
{
this.transformStampedMsg.SendMsg(this.networkStream);
this.isPublishing = false;
});
// sw.Stop();
// Debug.Log("tf sending time="+sw.Elapsed);
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//transforms
arraylength = rand.Next(10);
if (transforms == null)
{
transforms = new TransformStamped[arraylength];
}
else
{
Array.Resize(ref transforms, arraylength);
}
for (int i = 0; i < transforms.Length; i++)
{
//transforms[i]
transforms[i] = new TransformStamped();
transforms[i].Randomize();
}
}
void tfCb (TransformStamped transform)
{
header_ = transform.header;
pose_.position.x = transform.transform.translation.x;
pose_.position.y = transform.transform.translation.y;
pose_.position.z = transform.transform.translation.z;
pose_.orientation = transform.transform.rotation;
diff_.updateAndEstimate ( header_.Stamp, pose_, twist_, accel_ );
available_ = true;
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//transform
transform = new TransformStamped(SERIALIZEDSTUFF, ref currentIndex);
//error
error = new Messages.tf2_msgs.TF2Error(SERIALIZEDSTUFF, ref currentIndex);
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//transform
transform = new TransformStamped();
transform.Randomize();
//error
error = new Messages.tf2_msgs.TF2Error();
error.Randomize();
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
this.publishSequenceNumber = HSRPubSynchronizer.GetAssignedSequenceNumber();
}
protected virtual void PubTF()
{
// System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
// sw.Start();
this.isPublishing = true;
TransformStamped[] transformStampedArray = new TransformStamped[localTfInfoList.Count];
// Add local TF infos
for (int i = 0; i < localTfInfoList.Count; i++)
{
if (localTfInfoList[i].transformStamped.child_frame_id == this.baseFootprintName)
{
localTfInfoList[i].UpdateTransformForLocal(this.GetPosObservationNoise(), this.GetRotObservationNoise());
}
else
{
localTfInfoList[i].UpdateTransformForLocal();
}
localTfInfoList[i].transformStamped.header.Update();
transformStampedArray[i] = localTfInfoList[i].transformStamped;
}
this.transformStampedMsg.msg = transformStampedArray;
if (this.isUsingThread)
{
Thread thread = new Thread(new ThreadStart(SendTF));
thread.Start();
}
else
{
this.SendTF();
}
// sw.Stop();
// Debug.Log("tf sending time="+sw.Elapsed);
}
public void TestDynamicMessageWithDependencies()
{
const string fullDependencies = @"
Header header
string child_frame_id # the frame id of the child frame
geometry_msgs/Transform transform
================================================================================
MSG: std_msgs/Header
uint32 seq
time stamp
string frame_id
================================================================================
MSG: geometry_msgs/Transform
Vector3 translation
Quaternion rotation
================================================================================
MSG: geometry_msgs/Vector3
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Quaternion
float64 x
float64 y
float64 z
float64 w
";
DynamicMessage message =
DynamicMessage.CreateFromDependencyString("iviz_test/TransformStamped", fullDependencies);
var realTransformStamped = new TransformStamped();
Assert.True(message.RosInstanceMd5Sum == TransformStamped.RosMd5Sum);
Assert.True(message.RosMessageLength == realTransformStamped.RosMessageLength);
byte[] messageBytes = message.SerializeToArray();
byte[] otherMessageBytes = realTransformStamped.SerializeToArray();
Assert.True(messageBytes.SequenceEqual(otherMessageBytes));
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = TurtleBot3Common.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
this.synchronizer = this.GetComponent <TurtleBot3PubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
}
public override byte[] Serialize(bool partofsomethingelse)
{
int currentIndex = 0, length = 0;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
List <byte[]> pieces = new List <byte[]>();
GCHandle h;
//transforms
hasmetacomponents |= true;
if (transforms == null)
{
transforms = new TransformStamped[0];
}
pieces.Add(BitConverter.GetBytes(transforms.Length));
for (int i = 0; i < transforms.Length; i++)
{
//transforms[i]
if (transforms[i] == null)
{
transforms[i] = new TransformStamped();
}
pieces.Add(transforms[i].Serialize(true));
}
//combine every array in pieces into one array and return it
int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
int __a_b__e = 0;
byte[] __a_b__d = new byte[__a_b__f];
foreach (var __p__ in pieces)
{
Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
__a_b__e += __p__.Length;
}
return(__a_b__d);
}
protected List <UnityEngine.Transform> linkList; // link list of the robot
protected virtual void Awake()
{
this.InitializeVariables();
foreach (UnityEngine.Transform link in this.linkList)
{
if (link.name == this.baseFootprintName)
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = this.odomName;
localTransformStamped.child_frame_id = link.name;
UnityEngine.Transform baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, this.baseFootprintRigidbodyName);
TfInfo localTfInfo = new TfInfo(baseFootprintRigidbody, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = link.parent.name;
localTransformStamped.child_frame_id = link.name;
TfInfo localTfInfo = new TfInfo(link, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <RobotPubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
if (localLink.name == HSRCommon.Link.base_footprint.ToString())
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = HSRCommon.OdomName;
localTransformStamped.child_frame_id = localLink.name;
UnityEngine.Transform baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);
TfInfo localTfInfo = new TfInfo(baseFootprintRigidbody, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <HSRPubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
public LookupTransformResult(TransformStamped transform, TF2Error error)
{
this.transform = transform;
this.error = error;
}
protected override void ReceiveMessage(TransformStamped message)
{
lastMsg = message;
receivedMsg = true;
}
public TfInfo(UnityEngine.Transform linkTransform, TransformStamped transformStamped)
{
this.linkTransform = linkTransform;
this.transformStamped = transformStamped;
}
public FoundAnchor(string anchor_id, TransformStamped anchor_in_world_frame)
{
this.anchor_id = anchor_id;
this.anchor_in_world_frame = anchor_in_world_frame;
}
public FoundAnchor()
{
this.anchor_id = "";
this.anchor_in_world_frame = new TransformStamped();
}
///<exclude/>
public void Deserialize(BinaryReader br)
{
transforms = new List<TransformStamped>(br.ReadInt32()); for(int i=0; i<transforms.Capacity; i++) { var x = new TransformStamped(br);transforms.Add(x);}
}
public LookupTransformResult()
{
this.transform = new TransformStamped();
this.error = new TF2Error();
}
