Wait set groups

[sloretz]

Feature request

This is a request for an API on top of the current waitset api that considers groups of waitable entities.

Feature description

An API that covers groups of entities that can be waited on would be immediately useful for actions. A server and client consists of several waitable entities (servers, clients, maybe a timer). The API would allow a client library to add a group of waitable entities to the wait set, and then notify the action server when one of them becomes available.

Implementation considerations

This could be implemented in a package rcl_grouped_waitset on top of the existing rcl API

Structs and methods to be added.

struct rcl_grouped_waitset
{
  rcl_waitset_group_t * groups;
  size_t num_groups;

  rcl_waitset_group_impl_t * impl;
}

struct rcl_waitset_group_t
{
  rcl_subscription_t * subscriptions;
  rcl_guard_condition_t * guard_conditions;
  rcl_timer_t * timers;
  rcl_client_t * clients;
  rcl_service_t * servers;

  size_t num_subscriptions;
  size_t num_guard_conditions;
  size_t num_timers;
  size_t num_clients;
  size_t num_servers;
}
rcl_waitset_group_get_zero_initialized();

// User won't call these, but rcl_grouped_waitset_add and rcl_grouped_waitset_fini might
// rcl_waitset_group_init(rcl_waitset_group_t *, num_subs, subs, ..., rcl_allocator_t alloc);
// rcl_waitset_group_fini(rcl_waitset_group_t * g);

rcl_grouped_waitset_get_zero_initialized();
rcl_grouped_waitset_init(rcl_grouped_waitset_t * gws, size_t num_groups, rcl_allocator_t alloc);
rcl_grouped_waitset_fini(rcl_grouped_waitset_t * gws);

// Add a group waitable entities to the wait set
// This returns a waitset group that can be used to tell which entities in the group woke the wait set
// The returned group isn't usable until `rcl_grouped_waitset_build()` is called. The returned group never needs to be cleaned up by the user.
// subs, gcs, timers, etc pointers are borrowed and must be valid until `rcl_grouped_waitset_build()` is called
rcl_grouped_waitset_add(rcl_grouped_waitset_t * gws, rcl_waitset_group_t * g, size_t num_subs, rcl_subscription_t * subs, size_t num_guard_conditions, , rcl_guard_condition_t * gcs, size_t num_timers, rcl_timer_t * timers, size_t num_clients, rcl_client_t * clients, size_t num_servers, rcl_server_t * servers);

// Called after all groups have been added so allocation of the wait set only happens once
rcl_grouped_waitset_build();

// Calls `rcl_wait(rcl_waitset_t ws,...)` internally using a waitset built in `_build()` and stored in the implementation of the group object.
rcl_grouped_wait(rcl_grouped_waitset_t * gws, int64_t timeout);

Example use

rcl_waitset_group_t gws = rcl_grouped_waitset_get_zero_initialized();
rcl_grouped_waitset_init(&gws, num_groups, alloc);

// Client library builds a group with all of the users subs, clients, services, timers, etc
rcl_waitset_group_t cl_group = rcl_waitset_group_get_zero_initialized();
rcl_grouped_waitset_add(&gws, &cl_group, ...);

// For each action server client library asks it to add itself to the waitset group
{
  rcl_waitset_group group = rcl_waitset_group_get_zero_initialized();
  // This calls rcl_grouped_waitset_add() internally
  rcl_action_server_add_to_grouped_waitset(&as, &gws, &group);
}

rcl_grouped_wait(gws, ...)

// Entities are set to NULL within the added groups
for (size_t g = 0; g < gws.num_groups; ++g)
{
  const rcl_waitset_group_t * group = gws.groups[g];
  for (size_t s = 0; s < group->num_subscriptions; ++s)
  {
    // client library does right thing
    // First group could be user defined stuff,
    // Following groups could be for action servers, action clients, etc
  }
}

// When done finalizing the grouped waitset also cleans up the groups
// This works because rcl_grouped_waitset_build() set the pointers to the entities to be internal to an `rcl_waitset_t1
rcl_grouped_waitset_fini(gws);

connects to #307

[jacobperron]

@sloretz
It looks to me that the definition of rcl_waitset_group_t from your example very closely resembles rcl_wait_set_t:

rcl/rcl/include/rcl/wait.h

Lines 38 to 57 in a2546fa

	typedef struct rcl_wait_set_t
	{
	/// Storage for subscription pointers.
	const rcl_subscription_t ** subscriptions;
	size_t size_of_subscriptions;
	/// Storage for guard condition pointers.
	const rcl_guard_condition_t ** guard_conditions;
	size_t size_of_guard_conditions;
	/// Storage for timer pointers.
	const rcl_timer_t ** timers;
	size_t size_of_timers;
	/// Storage for client pointers.
	const rcl_client_t ** clients;
	size_t size_of_clients;
	/// Storage for service pointers.
	const rcl_service_t ** services;
	size_t size_of_services;
	/// Implementation specific storage.
	struct rcl_wait_set_impl_t * impl;
	} rcl_wait_set_t;

Rather than defining a new concept of a wait set group, maybe we could instead implement a second wait function that takes an array of rcl_wait_set_t, e.g.

rcl_wait_multiple(rcl_wait_set_t ** wait_sets, const size_t num_wait_sets, int64_t timeout);

I think this gives us the same capabilities but with less plumbing. What do you think?

[sloretz]

@jacobperron That could work. One thing to be aware of is the wait sets need to be consolidated before being passed to rmw_wait(). The impl pointer on the grouped waitset was a place to store those lists.

rcl/rcl/src/rcl/wait.c

Lines 543 to 549 in a2546fa

	rmw_ret_t ret = rmw_wait(
	&wait_set->impl->rmw_subscriptions,
	&wait_set->impl->rmw_guard_conditions,
	&wait_set->impl->rmw_services,
	&wait_set->impl->rmw_clients,
	wait_set->impl->rmw_wait_set,
	timeout_argument);

Maybe rcl_wait_multiple() could pick one of the wait sets to use as storage for the lists passed to rmw_wait()?

[jacobperron]

Maybe rcl_wait_multiple() could pick one of the wait sets to use as storage for the lists passed to rmw_wait()?

I guess we shouldn't modify the size of one of the input wait sets since the caller will check that after waiting. It doesn't seem like too much overhead to create a new wait set for the scope of the function where we collect all entities. Then we can call rcl_wait() on the collated wait set and propagate the update back to the input list.

[hidmic]

@jacobperron @sloretz I was just looking into rclcpp internals in preparation for the client implementation, and I started wondering if we'd be better off with just wait set grouping and ungrouping functions instead of adding another rcl_wait*() variant. Something like (or whatever name you guys prefer):

rcl_ret_t rcl_wait_set_group(const rcl_wait_set_t ** wait_sets, const size_t num_wait_sets, rcl_wait_set_t * grouped_wait_set);
rcl_ret_t rcl_wait_set_ungroup(const rcl_wait_set_t * grouped_wait_set, rcl_wait_set_t ** wait_sets, const size_t num_wait_sets);

Essentially splitting up the function @jacobperron wrote in three separate steps: grouping, waiting, ungrouping. I believe it'll make adapting client libraries simpler, see here and here.

[jacobperron]

@hidmic is it possible to provide an example/pseudocode of how you imagine these functions could be used in the context of actions?

[hidmic]

@jacobperron so, for a generic example, I'll just reuse the one you wrote for rcl_wait_multiple(), it's basically the same but in more steps:

#include <rcl/rcl.h>

// rcl_init() called successfully before here...
rcl_node_t node;  // initialize this, see rcl_node_init()
rcl_subscription_t sub1;  // initialize this, see rcl_subscription_init()
rcl_subscription_t sub2;  // initialize this, see rcl_subscription_init()
rcl_guard_condition_t gc1;  // initialize this, see rcl_guard_condition_init()
rcl_wait_set_t wait_sets[2];  // two wait sets in this example
wait_sets[0] = rcl_get_zero_initialized_wait_set();
wait_sets[1] = rcl_get_zero_initialized_wait_set();
rcl_ret_t ret = rcl_wait_set_init(&wait_sets[0] 1, 0, 0, 0, 0, rcl_get_default_allocator());
// ... error handling
ret = rcl_wait_set_init(&wait_sets[1] 1, 1, 0, 0, 0, rcl_get_default_allocator());
// ... error handling
do {
  ret = rcl_wait_set_clear(&wait_sets[0]);
  // ... error handling
  ret = rcl_wait_set_add_subscription(&wait_sets[0], &sub1);
  // ... error handling
  ret = rcl_wait_set_clear(&wait_sets[1]);
  // ... error handling
  ret = rcl_wait_set_add_subscription(&wait_sets[1], &sub2);
  // ... error handling
  ret = rcl_wait_set_add_guard_condition(&wait_sets[1], &gc1);
  // ... error handling
  rcl_wait_set_t grouped_wait_set = rcl_get_zero_initialized_wait_set();
  ret = rcl_wait_set_group(wait_sets, 2,&grouped_wait_set);
  // ... error handling
  ret = rcl_wait(grouped_wait_set, RCL_MS_TO_NS(1000));  // 1000ms == 1s, passed as ns
  if (ret == RCL_RET_TIMEOUT) {
    continue;
  }
  ret = rcl_wait_set_ungroup(&grouped_wait_set, wait_sets, 2);
  // ... error handling
  if (wait_sets[0].subscriptions[0]) {
   // The subscription in the first wait set is ready...
  }
  if (wait_sets[1].subscriptions[0]) {
   // The subscription in the second wait set is ready...
 }
  if (wait_sets[1].guard_conditions[0]) {
    // The guard condition in the first wait set is ready...
  }
} while(check_some_condition());
// ... fini node, and subscriptions and guard conditions...
ret = rcl_wait_set_fini(&wait_sets[0]);
// ... error handling
ret = rcl_wait_set_fini(&wait_sets[1]);
// ... error handling

The real benefit comes when updating rclcpp to work with action clients and servers. There, the grouped wait set is the wait set being passed in to add_handles_to_wait_set() and to remove_null_handles().

Actually, we can keep rcl_wait_multiple() (it's way more comfortable e.g. for an rclc user that just wants the thing to work) by implementing it in terms of these ones, like:

rcl_ret_t rcl_wait_multiple(rcl_wait_set_t wait_sets[], const size_t num_wait_sets, int64_t timeout) {
   rcl_wait_set_t grouped_wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_group(wait_sets, num_wait_sets, &grouped_wait_set);
   // ... error handling
   ret = rcl_wait(&group_wait_set, timeout);
   // ... error handling
   ret = rcl_wait_set_ungroup(&grouped_wait_set, wait_sets, num_wait_sets);
   // ... error handling
   return ret;
}

[hidmic]

Not a something we must do, I just thought it may be helpful considering API freeze is next Wednesday.

[jacobperron]

Not a something we must do, I just thought it may be helpful considering API freeze is next Wednesday.

Yeah, it doesn't seem like a bad idea. It might help with readability too since rcl_wait_mutliple() can reuse the grouping functions.

[jacobperron]

@hidmic How can the implementation of rcl_wait_set_ungroup() know what entities belong to which wait set?

Nevermind, I guess it only works if the input array of wait sets is the same as when rcl_wait_set_group was called.

[hidmic]

Yeap, it builds on top of the same underlying assumptions as rcl_wait_multiple()'s current implementation.

[jacobperron]

As an alternative solution for working with actions and wait sets, we can instead modify the existing wait set API to return the index of the entity in the wait set when it is added. For example, adding a service:

rcl_ret_t
rcl_wait_set_add_service(
  rcl_wait_set_t * wait_set,
  const rcl_service_t * service,
  size_t * index);

Then, the action server/client can have an implementation like so:

rcl_ret_t
rcl_action_wait_set_add_action_server(
  rcl_wait_set_t * wait_set,
  const rcl_action_server_t * action_server)                                                                                                               
{
  // ... input error handling
  rcl_ret_t ret = rcl_wait_set_add_service(                                                                                                                
    wait_set,
    &action_server->impl->goal_service,
    &action_server->impl->wait_set_goal_service_index);
  // ... error handling
  ret = rcl_wait_set_add_service(
    wait_set,
    &action_server->impl->cancel_service,                                                                                                                  
    &action_server->impl->wait_set_cancel_service_index);                                                                                                  
  // ... error handling
  ret = rcl_wait_set_add_service(
    wait_set,
    &action_server->impl->result_service,
    &action_server->impl->wait_set_result_service_index);
  // ... error handling
  return RCL_RET_OK;                                                                                                                                       
}

rcl_ret_t                                                                                                                                                  
rcl_action_server_wait_set_get_entities_ready(                                                                                                             
  const rcl_wait_set_t * wait_set,
  const rcl_action_server_t * action_server,
  bool * is_goal_request_ready,
  bool * is_cancel_request_ready,
  bool * is_result_request_ready)
{
  // ... input error handling
  const size_t goal_service_index = action_server->impl->wait_set_goal_service_index;
  const size_t cancel_service_index = action_server->impl->wait_set_cancel_service_index;
  const size_t result_service_index = action_server->impl->wait_set_result_service_index;
  *is_goal_request_ready = (NULL != wait_set->services[goal_service_index]);
  *is_cancel_request_ready = (NULL != wait_set->services[cancel_service_index]);
  *is_result_request_ready = (NULL != wait_set->services[result_service_index]);
  return RCL_RET_OK; 
}