Skip to content

Commit

Permalink
only try to take waitables if they are ready
Browse files Browse the repository at this point in the history 
This avoids doing unnecessary allocations of native ROS messages only to check
that there is no new data. This might be a good thing to do right now as with
guard_conditions there could be probably more times the waitset is ready to fire
some callbacks, so work should be minimized here especially.
  • Loading branch information
@hoffmann-stefan
hoffmann-stefan committed May 15, 2023
1 parent ecc2678 commit 7e52310
Show file tree
Hide file tree
Showing 3 changed files with 113 additions and 20 deletions.
91 changes: 71 additions & 20 deletions rcldotnet/RCLdotnet.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,21 @@ internal delegate RCLRet NativeRCLCreateWaitSetHandleType(

internal static NativeRCLWaitType native_rcl_wait = null;

[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
internal delegate bool NativeRCLWaitSetSubscriptionReady(SafeWaitSetHandle waitSetHandle, int index);

internal static NativeRCLWaitSetSubscriptionReady native_rcl_wait_set_subscription_ready = null;

[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
internal delegate bool NativeRCLWaitSetClientReady(SafeWaitSetHandle waitSetHandle, int index);

internal static NativeRCLWaitSetClientReady native_rcl_wait_set_client_ready = null;

[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
internal delegate bool NativeRCLWaitSetServiceReady(SafeWaitSetHandle waitSetHandle, int index);

internal static NativeRCLWaitSetServiceReady native_rcl_wait_set_service_ready = null;

[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
internal delegate bool NativeRCLWaitSetGuardConditionReady(SafeWaitSetHandle waitSetHandle, int index);

Expand Down Expand Up @@ -264,6 +279,24 @@ static RCLdotnetDelegates()
(NativeRCLWaitType)Marshal.GetDelegateForFunctionPointer(
native_rcl_wait_ptr, typeof(NativeRCLWaitType));

IntPtr native_rcl_wait_set_subscription_ready_ptr =
_dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_subscription_ready");
RCLdotnetDelegates.native_rcl_wait_set_subscription_ready =
(NativeRCLWaitSetSubscriptionReady)Marshal.GetDelegateForFunctionPointer(
native_rcl_wait_set_subscription_ready_ptr, typeof(NativeRCLWaitSetSubscriptionReady));

IntPtr native_rcl_wait_set_client_ready_ptr =
_dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_client_ready");
RCLdotnetDelegates.native_rcl_wait_set_client_ready =
(NativeRCLWaitSetClientReady)Marshal.GetDelegateForFunctionPointer(
native_rcl_wait_set_client_ready_ptr, typeof(NativeRCLWaitSetClientReady));

IntPtr native_rcl_wait_set_service_ready_ptr =
_dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_service_ready");
RCLdotnetDelegates.native_rcl_wait_set_service_ready =
(NativeRCLWaitSetServiceReady)Marshal.GetDelegateForFunctionPointer(
native_rcl_wait_set_service_ready_ptr, typeof(NativeRCLWaitSetServiceReady));

IntPtr native_rcl_wait_set_guard_condition_ready_ptr =
_dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_guard_condition_ready");
RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready =
Expand Down Expand Up @@ -640,55 +673,73 @@ public static void SpinOnce(Node node, long timeout)
return; // timeout
}

int subscriptionIndex = 0;
foreach (Subscription subscription in node.Subscriptions)
{
IRosMessage message = subscription.CreateMessage();
bool result = Take(subscription, message);
if (result)
if (RCLdotnetDelegates.native_rcl_wait_set_subscription_ready(waitSetHandle, subscriptionIndex))
{
subscription.TriggerCallback(message);
IRosMessage message = subscription.CreateMessage();
bool result = Take(subscription, message);
if (result)
{
subscription.TriggerCallback(message);
}
}

subscriptionIndex++;
}

// requestIdHandle gets reused for each element in the loop.
using (SafeRequestIdHandle requestIdHandle = CreateRequestId())
{
int serviceIndex = 0;
foreach (var service in node.Services)
{
var request = service.CreateRequest();
var response = service.CreateResponse();

var result = TakeRequest(service, requestIdHandle, request);
if (result)
if (RCLdotnetDelegates.native_rcl_wait_set_service_ready(waitSetHandle, serviceIndex))
{
service.TriggerCallback(request, response);
var request = service.CreateRequest();
var response = service.CreateResponse();

SendResponse(service, requestIdHandle, response);
var result = TakeRequest(service, requestIdHandle, request);
if (result)
{
service.TriggerCallback(request, response);

SendResponse(service, requestIdHandle, response);
}
}

serviceIndex++;
}

int clientIndex = 0;
foreach (var client in node.Clients)
{
var response = client.CreateResponse();

var result = TakeResponse(client, requestIdHandle, response);
if (result)
if (RCLdotnetDelegates.native_rcl_wait_set_client_ready(waitSetHandle, clientIndex))
{
var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle);
client.HandleResponse(sequenceNumber, response);
var response = client.CreateResponse();

var result = TakeResponse(client, requestIdHandle, response);
if (result)
{
var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle);
client.HandleResponse(sequenceNumber, response);
}
}

clientIndex++;
}
}

int index = 0;
int guardConditionIndex = 0;
foreach (GuardCondition guardCondition in node.GuardConditions)
{
if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, index))
if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, guardConditionIndex))
{
guardCondition.TriggerCallback();
}

index++;
guardConditionIndex++;
}
}
}
Expand Down
33 changes: 33 additions & 0 deletions rcldotnet/rcldotnet.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,39 @@ int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) {
return ret;
}

bool native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index) {
rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle;

if (index >= wait_set->size_of_subscriptions)
{
return false;
}

return wait_set->subscriptions[index] != NULL;
}

bool native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index) {
rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle;

if (index >= wait_set->size_of_clients)
{
return false;
}

return wait_set->clients[index] != NULL;
}

bool native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index) {
rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle;

if (index >= wait_set->size_of_services)
{
return false;
}

return wait_set->services[index] != NULL;
}

bool native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index) {
rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle;

Expand Down
9 changes: 9 additions & 0 deletions rcldotnet/rcldotnet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,15 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wai
RCLDOTNET_EXPORT
int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t);

RCLDOTNET_EXPORT
bool RCLDOTNET_CDECL native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index);

RCLDOTNET_EXPORT
bool RCLDOTNET_CDECL native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index);

RCLDOTNET_EXPORT
bool RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index);

RCLDOTNET_EXPORT
bool RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index);

Expand Down

0 comments on commit 7e52310

Please sign in to comment.