public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//header
header = new Messages.std_msgs.Header(serializedMessage, ref currentIndex);
//status
piecesize = Marshal.SizeOf(typeof(int));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
status = (int)Marshal.PtrToStructure(h, typeof(int));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//details
details = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
details = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
public override byte[] Serialize(bool partofsomethingelse)
{
List <byte[]> pieces = new List <byte[]>();
if (Header == null)
{
Header = new std_msgs.Header();
}
pieces.Add(Header.Serialize(true));
if (GoalStatus == null)
{
GoalStatus = new actionlib_msgs.GoalStatus();
}
pieces.Add(GoalStatus.Serialize(true));
if (Content == null)
{
Content = new T();
}
pieces.Add(Content.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 Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//header
header = new Messages.std_msgs.Header();
header.Randomize();
//status
status = rand.Next();
//details
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
details = Encoding.ASCII.GetString(strbuf);
}
// Update is called once per frame
void Update()
{
Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage();
Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1];
arr[0] = new Messages.geometry_msgs.TransformStamped();
tfmsg.transforms = arr;
Transform trans = trackedObject.transform;
emTransform ta = new emTransform(trans, ROS.GetTime(), frame_id, child_frame_id);
Messages.std_msgs.Header hdr = new Messages.std_msgs.Header();
hdr.frame_id = frame_id;
hdr.stamp = ROS.GetTime();
if (!using_gazebo)
{
hdr.stamp.data.sec += 18000;
}
tfmsg.transforms[0].header = hdr;
tfmsg.transforms[0].child_frame_id = child_frame_id;
tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform();
tfmsg.transforms[0].transform.translation = ta.origin.ToMsg();
//tfmsg.transforms[0].transform.translation.z += 1.0;
tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg();
tfmsg.Serialized = null;
tfPub.publish(tfmsg);
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
Header = new std_msgs.Header(serializedMessage, ref currentIndex);
GoalStatus = new actionlib_msgs.GoalStatus(serializedMessage, ref currentIndex);
//Content = (T)Activator.CreateInstance( typeof( T ), serializedMessage, currentIndex );
Content = (T)typeof(T).GetInstance();
Content.Deserialize(serializedMessage, ref currentIndex);
}
public OdomSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel acceleration, Header header)
{
pose_ = pose;
twist_ = twist;
acceleration_ = acceleration;
header_ = header;
odom_sub_ = nh.subscribe<Odometry> ( topic, 1, odomCallback );
}
void Update()
{
if (publishtf)
{
_tfmsg.transforms[0].header.stamp = ROS.GetTime();
_tfmsg.transforms[0].header.stamp.data.sec += 18000; //windows time is dumb
//Debug.Log("Current time" + ROS.GetTime().data.sec);
tfPub.publish(_tfmsg);
}
if (hj == null)
{
hj = GameObject.Find(frame_id);
}
else if (!interactableItem.IsInteracting())
{
if (!startedInteracting)
{
//Debug.Log("Not interacting!");
Vector3 t = hj.transform.position;
Quaternion q = hj.transform.rotation;
transform.position = t;
transform.rotation = q;
}
if (startedInteracting)
{
Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage();
Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1];
arr[0] = new Messages.geometry_msgs.TransformStamped();
tfmsg.transforms = arr;
//Transform trans = trackedObj.transform;
emTransform ta = new emTransform(transform, ROS.GetTime(), "/world", "/look_at_frame");
Messages.std_msgs.Header hdr = new Messages.std_msgs.Header();
hdr.frame_id = "/world";
hdr.stamp = ROS.GetTime();
hdr.stamp.data.sec += 18000;
tfmsg.transforms[0].header = hdr;
tfmsg.transforms[0].child_frame_id = "/look_at_frame";
tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform();
tfmsg.transforms[0].transform.translation = ta.origin.ToMsg();
tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg();
tfmsg.Serialized = null;
tfPub.publish(tfmsg);
_tfmsg = tfmsg;
publishtf = true;
startedInteracting = false;
}
}
else
{
startedInteracting = true;
}
}
public override void Randomize()
{
header = new Messages.std_msgs.Header();
header.Randomize();
goal_id = new Messages.actionlib_msgs.GoalID();
goal_id.Randomize();
goal = new LandingGoal();
goal.Randomize();
}
public StateSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel accel, Header header)
{
pose_ = pose;
twist_ = twist;
accel_ = accel;
header_ = header;
available_ = false;
tf_sub_ = nh.subscribe<TransformStamped> ( topic, 1, tfCb );
}
internal bool EnqueueMessage(MessageAndSerializerFunc holder)
{
lock (gate)
{
if (Dropped)
{
return(false);
}
}
uint seq = IncrementSequence();
if (HasHeader)
{
object h = holder.msg.GetType().GetTypeInfo().GetField("header").GetValue(holder.msg);
std_msgs.Header header;
if (h == null)
{
header = new std_msgs.Header();
}
else
{
header = (std_msgs.Header)h;
}
header.seq = seq;
if (header.stamp == null)
{
header.stamp = ROS.GetTime();
}
if (header.frame_id == null)
{
header.frame_id = "";
}
holder.msg.GetType().GetTypeInfo().GetField("header").SetValue(holder.msg, header);
}
holder.msg.connection_header = connectionHeader.Values;
lock (gate)
{
foreach (SubscriberLink link in subscriberLinks)
{
link.EnqueueMessage(holder);
}
}
if (Latch)
{
lastMessage = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
}
return(true);
}
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;
}
internal bool EnqueueMessage(MessageAndSerializerFunc holder)
{
lock (subscriber_links_mutex)
{
if (Dropped)
{
return(false);
}
}
uint seq = incrementSequence();
if (HasHeader)
{
object h = holder.msg.GetType().GetField("header").GetValue(holder.msg);
m.Header header;
if (h == null)
{
header = new m.Header();
}
else
{
header = (m.Header)h;
}
header.Seq = seq;
if (header.Stamp == null)
{
header.Stamp = ROS.GetTime();
}
if (header.Frame_id == null)
{
header.Frame_id = "";
}
holder.msg.GetType().GetField("header").SetValue(holder.msg, header);
}
holder.msg.connection_header = connection_header.Values;
lock (subscriber_links_mutex)
foreach (SubscriberLink sub_link in subscriber_links)
{
sub_link.enqueueMessage(holder);
}
if (Latch)
{
last_message = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
}
return(true);
}
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;
IntPtr ptr;
int x__size;
//header
if (header == null)
{
header = new Messages.std_msgs.Header();
}
pieces.Add(header.Serialize(true));
//status
scratch1 = new byte[Marshal.SizeOf(typeof(int))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(status, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//details
if (details == null)
{
details = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)details);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
// 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 bool EnqueueMessage(IRosMessage msg)
{
lock (subscriber_links_mutex)
{
if (Dropped)
{
return(false);
}
}
uint seq = incrementSequence();
if (HasHeader)
{
object h = msg.GetType().GetField("header").GetValue(msg);
m.Header header;
if (h == null)
{
header = new m.Header();
}
else
{
header = (m.Header)h;
}
header.seq = seq;
header.stamp = ROS.GetTime();
header.frame_id = new m.String();
msg.GetType().GetField("header").SetValue(msg, header);
}
msg.connection_header = connection_header.Values;
lock (subscriber_links_mutex)
foreach (SubscriberLink sub_link in subscriber_links)
{
sub_link.enqueueMessage(msg, true, false);
}
if (Latch)
{
last_message = msg;
}
return(true);
}
void odomCallback (Odometry odom)
{
// calculate acceleration
if ( header_.Stamp.data.toSec () != 0.0 && odom.header.Stamp.data.toSec () != 0.0 )
{
double acceleration_time_constant = 0.1;
double dt = ( ( odom.header.Stamp - header_.Stamp ).data.toSec () );
if ( dt > 0.0 )
{
acceleration_.linear.x = ( ( odom.twist.twist.linear.x - twist_.linear.x ) + acceleration_time_constant * acceleration_.linear.x ) / ( dt + acceleration_time_constant );
acceleration_.linear.y = ( ( odom.twist.twist.linear.y - twist_.linear.y ) + acceleration_time_constant * acceleration_.linear.y ) / ( dt + acceleration_time_constant );
acceleration_.linear.z = ( ( odom.twist.twist.linear.z - twist_.linear.z ) + acceleration_time_constant * acceleration_.linear.z ) / ( dt + acceleration_time_constant );
acceleration_.angular.x = ( ( odom.twist.twist.angular.x - twist_.angular.x ) + acceleration_time_constant * acceleration_.angular.x ) / ( dt + acceleration_time_constant );
acceleration_.angular.y = ( ( odom.twist.twist.angular.y - twist_.angular.y ) + acceleration_time_constant * acceleration_.angular.y ) / ( dt + acceleration_time_constant );
acceleration_.angular.z = ( ( odom.twist.twist.angular.z - twist_.angular.z ) + acceleration_time_constant * acceleration_.angular.z ) / ( dt + acceleration_time_constant );
}
}
header_ = odom.header;
pose_ = odom.pose.pose;
twist_ = odom.twist.twist;
}
internal bool EnqueueMessage(MessageAndSerializerFunc holder)
{
lock (subscriber_links_mutex)
{
if (Dropped) return false;
}
uint seq = incrementSequence();
if (HasHeader)
{
object h = holder.msg.GetType().GetField("header").GetValue(holder.msg);
m.Header header;
if (h == null)
header = new m.Header();
else
header = (m.Header) h;
header.seq = seq;
if (header.stamp == null)
{
header.stamp = ROS.GetTime();
}
if (header.frame_id == null)
{
header.frame_id = "";
}
holder.msg.GetType().GetField("header").SetValue(holder.msg, header);
}
holder.msg.connection_header = connection_header.Values;
lock (subscriber_links_mutex)
foreach (SubscriberLink sub_link in subscriber_links)
{
sub_link.enqueueMessage(holder);
}
if (Latch)
{
last_message = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
}
return true;
}
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;
IntPtr ptr;
int x__size;
//header
if (header == null)
{
header = new Messages.std_msgs.Header();
}
pieces.Add(header.Serialize(true));
//system_status
scratch1 = new byte[Marshal.SizeOf(typeof(int))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(system_status, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//topics
hasmetacomponents |= false;
if (topics == null)
{
topics = new string[0];
}
pieces.Add(BitConverter.GetBytes(topics.Length));
for (int i = 0; i < topics.Length; i++)
{
//topics[i]
if (topics[i] == null)
{
topics[i] = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)topics[i]);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
}
//err_code
hasmetacomponents |= false;
if (err_code == null)
{
err_code = new int[0];
}
pieces.Add(BitConverter.GetBytes(err_code.Length));
// Start Xamla
//err_code
x__size = Marshal.SizeOf(typeof(int)) * err_code.Length;
scratch1 = new byte[x__size];
Buffer.BlockCopy(err_code, 0, scratch1, 0, x__size);
pieces.Add(scratch1);
// End Xamla
//topic_msg
hasmetacomponents |= false;
if (topic_msg == null)
{
topic_msg = new string[0];
}
pieces.Add(BitConverter.GetBytes(topic_msg.Length));
for (int i = 0; i < topic_msg.Length; i++)
{
//topic_msg[i]
if (topic_msg[i] == null)
{
topic_msg[i] = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)topic_msg[i]);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
}
// 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 Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//header
header = new Messages.std_msgs.Header();
header.Randomize();
//system_status
system_status = rand.Next();
//topics
arraylength = rand.Next(10);
if (topics == null)
{
topics = new string[arraylength];
}
else
{
Array.Resize(ref topics, arraylength);
}
for (int i = 0; i < topics.Length; i++)
{
//topics[i]
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
topics[i] = Encoding.ASCII.GetString(strbuf);
}
//err_code
arraylength = rand.Next(10);
if (err_code == null)
{
err_code = new int[arraylength];
}
else
{
Array.Resize(ref err_code, arraylength);
}
for (int i = 0; i < err_code.Length; i++)
{
//err_code[i]
err_code[i] = rand.Next();
}
//topic_msg
arraylength = rand.Next(10);
if (topic_msg == null)
{
topic_msg = new string[arraylength];
}
else
{
Array.Resize(ref topic_msg, arraylength);
}
for (int i = 0; i < topic_msg.Length; i++)
{
//topic_msg[i]
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
topic_msg[i] = Encoding.ASCII.GetString(strbuf);
}
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//header
header = new Messages.std_msgs.Header(serializedMessage, ref currentIndex);
//system_status
piecesize = Marshal.SizeOf(typeof(int));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
system_status = (int)Marshal.PtrToStructure(h, typeof(int));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//topics
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (topics == null)
{
topics = new string[arraylength];
}
else
{
Array.Resize(ref topics, arraylength);
}
for (int i = 0; i < topics.Length; i++)
{
//topics[i]
topics[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
topics[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
//err_code
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (err_code == null)
{
err_code = new int[arraylength];
}
else
{
Array.Resize(ref err_code, arraylength);
}
// Start Xamla
//err_code
piecesize = Marshal.SizeOf(typeof(int)) * err_code.Length;
if (currentIndex + piecesize > serializedMessage.Length)
{
throw new Exception("Memory allocation failed: Ran out of bytes to read.");
}
Buffer.BlockCopy(serializedMessage, currentIndex, err_code, 0, piecesize);
currentIndex += piecesize;
// End Xamla
//topic_msg
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (topic_msg == null)
{
topic_msg = new string[arraylength];
}
else
{
Array.Resize(ref topic_msg, arraylength);
}
for (int i = 0; i < topic_msg.Length; i++)
{
//topic_msg[i]
topic_msg[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
topic_msg[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
header = new Messages.std_msgs.Header(SERIALIZEDSTUFF, ref currentIndex);
goal_id = new Messages.actionlib_msgs.GoalID(SERIALIZEDSTUFF, ref currentIndex);
goal = new LandingGoal(SERIALIZEDSTUFF, ref currentIndex);
}
