Skip to content

Commit

Permalink
Remove subscription (#106) (#113)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mergify
mergify[bot] authored Jul 9, 2021
1 parent ae58a72 commit 0263722
Show file tree
Hide file tree
Showing 3 changed files with 683 additions and 0 deletions.
135 changes: 135 additions & 0 deletions rclc/include/rclc/executor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -434,6 +434,141 @@ rclc_executor_add_guard_condition(
rcl_guard_condition_t * gc,
rclc_gc_callback_t callback);


/**
* Removes a subscription from an executor.
* * An error is returned if {@link rclc_executor_t.handles} array is empty.
* * An error is returned if subscription is not found in {@link rclc_executor_t.handles}.
* * The total number_of_subscriptions field of {@link rclc_executor_t.info}
* is decremented by one.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param [inout] executor pointer to initialized executor
* \param [in] subscription pointer to an allocated and initialized subscription previously added to executor
* \return `RCL_RET_OK` if add-operation was successful
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
rcl_ret_t
rclc_executor_remove_subscription(
rclc_executor_t * executor,
const rcl_subscription_t * subscription);


/**
* Removes a timer from an executor.
* * An error is returned if {@link rclc_executor_t.handles} array is empty.
* * An error is returned if timer is not found in {@link rclc_executor_t.handles}.
* * The total number_of_timers field of {@link rclc_executor_t.info}
* is incremented by one.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param [inout] executor pointer to initialized executor
* \param [in] timer pointer to an allocated and initialized timer previously added to executor
* \return `RCL_RET_OK` if add-operation was successful
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
rcl_ret_t
rclc_executor_remove_timer(
rclc_executor_t * executor,
const rcl_timer_t * timer);


/**
* Removes a client from an executor.
* * An error is returned if {@link rclc_executor_t.handles} array is empty.
* * An error is returned if client is not found in {@link rclc_executor_t.handles}.
* * The total number_of_clients field of {@link rclc_executor_t.info}
* is incremented by one.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param [inout] executor pointer to initialized executor
* \param [in] client pointer to an allocated and initialized client previously added to executor
* \return `RCL_RET_OK` if add-operation was successful
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
rcl_ret_t
rclc_executor_remove_client(
rclc_executor_t * executor,
const rcl_client_t * client);


/**
* Removes a service from an executor.
* * An error is returned if {@link rclc_executor_t.handles} array is empty.
* * An error is returned if service is not found in {@link rclc_executor_t.handles}.
* * The total number_of_services field of {@link rclc_executor_t.info}
* is incremented by one.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param [inout] executor pointer to initialized executor
* \param [in] service pointer to an allocated and initialized service previously added to executor
* \return `RCL_RET_OK` if add-operation was successful
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
rcl_ret_t
rclc_executor_remove_service(
rclc_executor_t * executor,
const rcl_service_t * service);

/**
* Removes a guard_condition from an executor.
* * An error is returned if {@link rclc_executor_t.handles} array is empty.
* * An error is returned if guard_condition is not found in {@link rclc_executor_t.handles}.
* * The total number_of_guard_conditions field of {@link rclc_executor_t.info}
* is incremented by one.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param [inout] executor pointer to initialized executor
* \param [in] guard_condition pointer to an allocated and initialized guard_condition previously added to executor
* \return `RCL_RET_OK` if add-operation was successful
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
rcl_ret_t
rclc_executor_remove_guard_condition(
rclc_executor_t * executor,
const rcl_guard_condition_t * guard_condition);


/**
* The spin-some function checks one-time for new data from the DDS-queue.
* * the timeout is defined in {@link rclc_executor_t.timeout_ns} and can
Expand Down
175 changes: 175 additions & 0 deletions rclc/src/rclc/executor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -571,6 +571,181 @@ rclc_executor_add_guard_condition(
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Added a guard_condition.");
return ret;
}


rcl_ret_t
_rclc_executor_remove_handle(rclc_executor_t * executor, size_t handle_index)
{
RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = RCL_RET_OK;

if (handle_index >= executor->index) {
RCL_SET_ERROR_MSG("Handle out of bounds");
return RCL_RET_ERROR;
}
if (0 == executor->index) {
RCL_SET_ERROR_MSG("No handles to remove");
return RCL_RET_ERROR;
}

// shorten the list of handles without changing the order of remaining handles
executor->index--;
for (size_t i = handle_index; i < executor->index; i++) {
executor->handles[i] = executor->handles[i + 1];
}
ret = rclc_executor_handle_init(&executor->handles[executor->index], executor->max_handles);

// force a refresh of the wait set
if (rcl_wait_set_is_valid(&executor->wait_set)) {
ret = rcl_wait_set_fini(&executor->wait_set);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("Could not reset wait_set in _rclc_executor_remove_handle.");
return ret;
}
}

RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a handle.");
return ret;
}


rcl_ret_t
rclc_executor_remove_subscription(
rclc_executor_t * executor,
const rcl_subscription_t * subscription)
{
RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = RCL_RET_OK;

for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) {
if (SUBSCRIPTION == executor->handles[i].type) {
if (subscription == executor->handles[i].subscription) {
ret = _rclc_executor_remove_handle(executor, i);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_subscription.");
return ret;
}
executor->info.number_of_subscriptions--;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a subscription.");
return ret;
}
}
}
RCL_SET_ERROR_MSG("Subscription not found in rclc_executor_remove_subscription");
return RCL_RET_ERROR;
}

rcl_ret_t
rclc_executor_remove_timer(
rclc_executor_t * executor,
const rcl_timer_t * timer)
{
RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = RCL_RET_OK;

for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) {
if (TIMER == executor->handles[i].type) {
if (timer == executor->handles[i].timer) {
_rclc_executor_remove_handle(executor, i);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_timer.");
return ret;
}
executor->info.number_of_timers--;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a timer.");
return ret;
}
}
}
RCL_SET_ERROR_MSG("Timer not found in rclc_executor_remove_timer");
return RCL_RET_ERROR;
}

rcl_ret_t
rclc_executor_remove_client(
rclc_executor_t * executor,
const rcl_client_t * client)
{
RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = RCL_RET_OK;

for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) {
if (CLIENT == executor->handles[i].type) {
if (client == executor->handles[i].client) {
_rclc_executor_remove_handle(executor, i);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_client.");
return ret;
}
executor->info.number_of_clients--;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a client.");
return ret;
}
}
}
RCL_SET_ERROR_MSG("Client not found in rclc_executor_remove_client");
return RCL_RET_ERROR;
}

rcl_ret_t
rclc_executor_remove_service(
rclc_executor_t * executor,
const rcl_service_t * service)
{
RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = RCL_RET_OK;

for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) {
if (SERVICE == executor->handles[i].type) {
if (service == executor->handles[i].service) {
_rclc_executor_remove_handle(executor, i);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_service.");
return ret;
}
executor->info.number_of_services--;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a service.");
return ret;
}
}
}
RCL_SET_ERROR_MSG("Service not found in rclc_executor_remove_service");
return RCL_RET_ERROR;
}


rcl_ret_t
rclc_executor_remove_guard_condition(
rclc_executor_t * executor,
const rcl_guard_condition_t * guard_condition)
{
RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = RCL_RET_OK;

for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) {
if (GUARD_CONDITION == executor->handles[i].type) {
if (guard_condition == executor->handles[i].gc) {
_rclc_executor_remove_handle(executor, i);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_guard_condition.");
return ret;
}
executor->info.number_of_guard_conditions--;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a guard condition.");
return ret;
}
}
}
RCL_SET_ERROR_MSG("Guard Condition not found in rclc_executor_remove_guard_condition");
return RCL_RET_ERROR;
}


/***
* operates on handle executor->handles[i]
* - evaluates the status bit in the wait_set for this handle
Expand Down
Loading

0 comments on commit 0263722

Please sign in to comment.