public static bool Take(ISubscriptionBase subscription, IRclcsMessage message)
{
rcl_subscription_t subscription_handle = subscription.Handle;
IntPtr message_handle = message.Handle;
RCLReturnEnum ret = (RCLReturnEnum)NativeMethods.rcl_take(ref subscription_handle, message_handle, IntPtr.Zero);
return(ret == RCLReturnEnum.RCL_RET_OK);
}
public static void SpinOnce(INode node, double timeoutSec)
{
// NOTE(sam): Only single thread, add support for other executors?
if (timeoutSec > 0.0001d)
{
WaitSet waitSet = new WaitSet(node.Subscriptions);
waitSet.Wait(timeoutSec);
}
foreach (ISubscriptionBase subscription in node.Subscriptions)
{
IRclcsMessage message = subscription.CreateMessage();
bool gotMessage = Take(subscription, message);
if (gotMessage)
{
subscription.TriggerCallback(message);
}
}
}
public void TriggerCallback(IRclcsMessage message)
{
callback((T)message);
}