Micro-optimizations in rcl (#965)

* Only check wait_set->impl for NULL pointer.

The previous line already checked wait_set for NULL pointer,
so there is no reason to call rcl_wait_set_is_valid and
recheck that pointer.

* Remove some debug statements from rcl_wait.

While profiling an application, I found that a lot of time was
spent just checking whether a log message should be output
or not based on the current debug level.  None of the individual
calls are expensive, but since rcl_wait is called so often,
it ends up showing up in the profile.

This patch somewhat controversially removes the debug statements
from rcl_wait().  My view on it is that the utility of these
is fairly low (if you ever turned them on, you would be flooded
with information), and the cost is relatively high.  If you
are really debugging this stuff, you can add in your own, more
targeted debug statements or use a debugger.

Signed-off-by: Chris Lalancette <[email protected]>

rcl/src/rcl/wait.c

@@ -217,7 +217,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
 
 #define SET_ADD(Type) \
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
-  if (!rcl_wait_set_is_valid(wait_set)) { \
+  if (!wait_set->impl) { \
     RCL_SET_ERROR_MSG("wait set is invalid"); \
     return RCL_RET_WAIT_SET_INVALID; \
   } \
@@ -595,15 +595,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     temporary_timeout_storage.nsec = min_timeout % 1000000000;
     timeout_argument = &temporary_timeout_storage;
   }
-  RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
-    !timeout_argument, ROS_PACKAGE_NAME, "Waiting without timeout");
-  RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
-    timeout_argument, ROS_PACKAGE_NAME,
-    "Waiting with timeout: %" PRIu64 "s + %" PRIu64 "ns",
-    temporary_timeout_storage.sec, temporary_timeout_storage.nsec);
-  RCUTILS_LOG_DEBUG_NAMED(
-    ROS_PACKAGE_NAME, "Timeout calculated based on next scheduled timer: %s",
-    is_timer_timeout ? "true" : "false");
 
   // Wait.
   rmw_ret_t ret = rmw_wait(
@@ -630,7 +621,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     if (ret != RCL_RET_OK) {
       return ret;  // The rcl error state should already be set.
     }
-    RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Timer in wait set is ready");
     if (!is_ready) {
       wait_set->timers[i] = NULL;
     }
@@ -643,41 +633,34 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
   // Set corresponding rcl subscription handles NULL.
   for (i = 0; i < wait_set->size_of_subscriptions; ++i) {
     bool is_ready = wait_set->impl->rmw_subscriptions.subscribers[i] != NULL;
-    RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
-      is_ready, ROS_PACKAGE_NAME, "Subscription in wait set is ready");
     if (!is_ready) {
       wait_set->subscriptions[i] = NULL;
     }
   }
   // Set corresponding rcl guard_condition handles NULL.
   for (i = 0; i < wait_set->size_of_guard_conditions; ++i) {
     bool is_ready = wait_set->impl->rmw_guard_conditions.guard_conditions[i] != NULL;
-    RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
-      is_ready, ROS_PACKAGE_NAME, "Guard condition in wait set is ready");
     if (!is_ready) {
       wait_set->guard_conditions[i] = NULL;
     }
   }
   // Set corresponding rcl client handles NULL.
   for (i = 0; i < wait_set->size_of_clients; ++i) {
     bool is_ready = wait_set->impl->rmw_clients.clients[i] != NULL;
-    RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Client in wait set is ready");
     if (!is_ready) {
       wait_set->clients[i] = NULL;
     }
   }
   // Set corresponding rcl service handles NULL.
   for (i = 0; i < wait_set->size_of_services; ++i) {
     bool is_ready = wait_set->impl->rmw_services.services[i] != NULL;
-    RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Service in wait set is ready");
     if (!is_ready) {
       wait_set->services[i] = NULL;
     }
   }
   // Set corresponding rcl event handles NULL.
   for (i = 0; i < wait_set->size_of_events; ++i) {
     bool is_ready = wait_set->impl->rmw_events.events[i] != NULL;
-    RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Event in wait set is ready");
     if (!is_ready) {
       wait_set->events[i] = NULL;
     }