static extern void publish(int uid, rosGameObject rGO);
void LateUpdate()
{
int frame_count = Time.frameCount;
double time = Time.time;
#region ROSCsereal (Must be last!)
if (!serialConnected)
{
return;
}
// Spin for subscribers, should be blocking
spinOnce();
// Publish topics
foreach (var rp in registeredPublishers)
{
if (rp.Value == null)
{
continue;
}
rosGameObject rGO = new rosGameObject();
// Headers
rGO.unique_id = rp.Key;
rGO.frame_count = frame_count;
rGO.time = time;
// Parent (Immutable)
rGO.parent = Marshal.StringToCoTaskMemAnsi(rp.Value.parent);
// Poses (Transforms)
rGO.poses_length = (uint)rp.Value.poses.Count;
IntPtr posesPtr = Marshal.AllocCoTaskMem(Marshal.SizeOf(new unityPose()) * (int)rGO.poses_length);
if (rGO.poses_length > 0)
{
double[] temp = new double[7 * rGO.poses_length];
for (int i = 0; i != rGO.poses_length; ++i)
{
temp[7 * i] = rp.Value.poses[i].position.x;
temp[7 * i + 1] = rp.Value.poses[i].position.y;
temp[7 * i + 2] = rp.Value.poses[i].position.z;
temp[7 * i + 3] = rp.Value.poses[i].rotation.x;
temp[7 * i + 4] = rp.Value.poses[i].rotation.y;
temp[7 * i + 5] = rp.Value.poses[i].rotation.z;
temp[7 * i + 6] = rp.Value.poses[i].rotation.w;
}
Marshal.Copy(temp, 0, posesPtr, 7 * (int)rGO.poses_length);
}
rGO.poses = posesPtr;
// Events
rGO.events_length = (uint)rp.Value.events.Count;
rGO.has_events = rGO.events_length > 0 ? true : false; // Shorthand because I'm cool
IntPtr[] tempEventsPtrArray = new IntPtr [rGO.events_length];
if (rGO.has_events && rGO.events_length > 0)
{
rGO.events = Marshal.AllocCoTaskMem(Marshal.SizeOf(new IntPtr()) * rp.Value.events.Count);
int iterEvents = 0;
foreach (var _event in rp.Value.events)
{
tempEventsPtrArray[iterEvents] = Marshal.StringToCoTaskMemAnsi(_event);
iterEvents++;
}
Marshal.Copy(tempEventsPtrArray, 0, rGO.events, (int)rGO.events_length);
}
// Values (Attributes or whatever)
rGO.values_length = (uint)rp.Value.values.val.Count;
rGO.has_values = rGO.values_length > 0 ? true : false; // Shorthand because I'm cool
// NOTE: Do not name variables "value".
rGO.values_name = Marshal.AllocCoTaskMem(Marshal.SizeOf(new IntPtr()) * (int)rGO.values_length);
IntPtr[] tempValuesNameArray = new IntPtr [rGO.values_length];
rGO.values_data = Marshal.AllocCoTaskMem(Marshal.SizeOf(new double()) * (int)rGO.values_length);
double[] tempValuesDataArray = new double [rGO.values_length];
int iterValues = 0;
foreach (var val in rp.Value.values.val)
{
tempValuesNameArray[iterValues] = Marshal.StringToCoTaskMemAnsi(val.Key);
tempValuesDataArray[iterValues] = val.Value;
iterValues++;
}
Marshal.Copy(tempValuesNameArray, 0, rGO.values_name, (int)rGO.values_length);
Marshal.Copy(tempValuesDataArray, 0, rGO.values_data, (int)rGO.values_length);
// Publishing
publish(rp.Key, rGO);
// Housekeeping
rp.Value.clearEvents();
Marshal.FreeCoTaskMem(posesPtr);
foreach (var item in tempEventsPtrArray)
{
Marshal.FreeCoTaskMem(item);
}
foreach (var item in tempValuesNameArray)
{
Marshal.FreeCoTaskMem(item);
}
Marshal.FreeCoTaskMem(rGO.values_data);
Marshal.FreeCoTaskMem(rGO.values_name);
}
spinOnce();
#endregion
}
void Update()
{
if (!serialConnected)
{
return;
}
spinOnce();
foreach (var sub in registeredSubscribers)
{
if (sub.Value == null)
{
continue;
}
rosGameObject _recv = new rosGameObject();
sub.Value.clearEvents();
try
{
Marshal.PtrToStructure(fetch(sub.Key), _recv);
}
catch (Exception ex)
{
continue;
}
sub.Value.parent = Marshal.PtrToStringAnsi(_recv.parent);
IntPtr[] _poses = new IntPtr [_recv.poses_length];
if (_recv.poses_length > 0)
{
Marshal.Copy(_recv.poses, _poses, 0, (int)_recv.poses_length);
}
for (int i = 0; i < _recv.poses_length; ++i)
{
// Only update what we can
if (i > sub.Value.poses.Count - 1)
{
continue;
}
unityPose _pose = new unityPose();
Marshal.PtrToStructure(_poses[i], _pose);
sub.Value.poses[i].position = new Vector3((float)_pose.px, (float)_pose.py, (float)_pose.pz);
sub.Value.poses[i].rotation = new Quaternion((float)_pose.ox, (float)_pose.oy, (float)_pose.oz, (float)_pose.ow);
}
IntPtr[] _events_ptr = new IntPtr[_recv.events_length];
if (_recv.events_length > 0)
{
Marshal.Copy(_recv.events, _events_ptr, 0, (int)_recv.events_length);
}
for (int i = 0; i < _recv.events_length; ++i)
{
sub.Value.logEvent(Marshal.PtrToStringAnsi(_events_ptr[i]));
}
double[] _values_data = new double[_recv.values_length];
IntPtr[] _values_name_ptr = new IntPtr[_recv.values_length];
if (_recv.values_length > 0)
{
Marshal.Copy(_recv.values_name, _values_name_ptr, 0, (int)_recv.values_length);
Marshal.Copy(_recv.values_data, _values_data, 0, (int)_recv.values_length);
}
for (int i = 0; i < _recv.values_length; ++i)
{
string name = Marshal.PtrToStringAnsi(_values_name_ptr[i]);
if (!sub.Value.values.setValue(name, _values_data[i]))
{
ROSSubscriber.logSubscriberError("Could not find value named: " + name);
}
}
}
}
