Add debug logging (#187)

* use ROS_PACKAGE_NAME in debug msgs

* rcl_lifecycle too

* Swap unnamed macros to named

* Remove semicolon

* Add debug logging

* Timer debug logging

* Wait debug

* A bit less wait debug...

* Clearer time output

* Remove the wait sublogger

* Use conditional logging instead of the else{}

* Add 'X finalized' msg

* Add send_response logging

* Remove extra semicolons

* Add publish/take messages

* [style nitpick] formatted variables on the next line

rcl/include/rcl/expand_topic_name.h

@@ -64,7 +64,7 @@ extern "C"
  * if (ret != RCL_RET_OK) {
  *   // ... error handling
  * } else {
- *   RCUTILS_LOG_INFO("Expanded topic name: %s", expanded_topic_name)
+ *   RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Expanded topic name: %s", expanded_topic_name)
  *   // ... when done the output topic name needs to be deallocated:
  *   allocator.deallocate(expanded_topic_name, allocator.state);
  * }

rcl/src/rcl/client.c

@@ -67,6 +67,8 @@ rcl_client_init(
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Initializing client for service name '%s'", service_name)
   if (client->impl) {
     RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized", *allocator);
     return RCL_RET_ALREADY_INIT;
@@ -87,7 +89,7 @@ rcl_client_init(
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
-        "rcl",
+        ROS_PACKAGE_NAME,
         "failed to fini string_map (%d) during error handling: %s\n",
         rcutils_ret,
         rcutils_get_error_string_safe())
@@ -120,6 +122,7 @@ rcl_client_init(
       return RCL_RET_ERROR;
     }
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name)
   // Validate the expanded service name.
   int validation_result;
   rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL);
@@ -151,6 +154,7 @@ rcl_client_init(
   }
   // options
   client->impl->options = *options;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized")
   return RCL_RET_OK;
 fail:
   if (client->impl) {
@@ -163,6 +167,7 @@ rcl_ret_t
 rcl_client_fini(rcl_client_t * client, rcl_node_t * node)
 {
   (void)node;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing client")
   rcl_ret_t result = RCL_RET_OK;
   RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@@ -182,6 +187,7 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node)
     }
     allocator.deallocate(client->impl, allocator.state);
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client finalized")
   return result;
 }
 
@@ -232,6 +238,7 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client sending service request")
   if (!rcl_client_is_valid(client)) {
     return RCL_RET_CLIENT_INVALID;
   }
@@ -257,6 +264,7 @@ rcl_take_response(
   rmw_request_id_t * request_header,
   void * ros_response)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response")
   if (!rcl_client_is_valid(client)) {
     return RCL_RET_CLIENT_INVALID;
   }
@@ -272,6 +280,8 @@ rcl_take_response(
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), client->impl->options.allocator);
     return RCL_RET_ERROR;
   }
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Client take response succeeded: %s", taken ? "true" : "false")
   if (!taken) {
     return RCL_RET_CLIENT_TAKE_FAILED;
   }

rcl/src/rcl/node.c

@@ -108,6 +108,8 @@ rcl_node_init(
   RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT, *allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT, *allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Initializing node '%s' in namespace '%s'", name, namespace_)
   if (node->impl) {
     RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized", *allocator);
     return RCL_RET_ALREADY_INIT;
@@ -215,6 +217,7 @@ rcl_node_init(
   }
   // actual domain id
   node->impl->actual_domain_id = domain_id;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Using domain ID of '%zu'", domain_id)
 
   const char * ros_security_enable = NULL;
   const char * ros_enforce_security = NULL;
@@ -230,6 +233,8 @@ rcl_node_init(
   }
 
   bool use_security = (0 == strcmp(ros_security_enable, "true"));
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Using security: %s", use_security ? "true" : "false")
 
   if (rcutils_get_env(ROS_SECURITY_STRATEGY_VAR_NAME, &ros_enforce_security)) {
     RCL_SET_ERROR_MSG(
@@ -301,14 +306,15 @@ rcl_node_init(
     // error message already set
     goto fail;
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized")
   return RCL_RET_OK;
 fail:
   if (node->impl) {
     if (node->impl->rmw_node_handle) {
       ret = rmw_destroy_node(node->impl->rmw_node_handle);
       if (ret != RMW_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
-          "rcl",
+          ROS_PACKAGE_NAME,
           "failed to fini rmw node in error recovery: %s", rmw_get_error_string_safe()
         )
       }
@@ -317,7 +323,7 @@ rcl_node_init(
       ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
       if (ret != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
-          "rcl",
+          ROS_PACKAGE_NAME,
           "failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe()
         )
       }
@@ -337,6 +343,7 @@ rcl_node_init(
 rcl_ret_t
 rcl_node_fini(rcl_node_t * node)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing node")
   RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!node->impl) {
     // Repeat calls to fini or calling fini on a zero initialized node is ok.
@@ -358,6 +365,7 @@ rcl_node_fini(rcl_node_t * node)
   // assuming that allocate and deallocate are ok since they are checked in init
   allocator.deallocate(node->impl, allocator.state);
   node->impl = NULL;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized")
   return result;
 }
 

rcl/src/rcl/publisher.c

@@ -70,6 +70,8 @@ rcl_publisher_init(
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Initializing publisher for topic name '%s'", topic_name)
   // Expand the given topic name.
   rcutils_allocator_t rcutils_allocator = *allocator;  // implicit conversion to rcutils version
   rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
@@ -86,7 +88,7 @@ rcl_publisher_init(
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
-        "rcl",
+        ROS_PACKAGE_NAME,
         "failed to fini string_map (%d) during error handling: %s",
         rcutils_ret,
         rcutils_get_error_string_safe())
@@ -119,6 +121,7 @@ rcl_publisher_init(
       return RCL_RET_ERROR;
     }
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name)
   // Validate the expanded topic name.
   int validation_result;
   rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL);
@@ -148,6 +151,7 @@ rcl_publisher_init(
     rmw_get_error_string_safe(), goto fail, *allocator);
   // options
   publisher->impl->options = *options;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized")
   return RCL_RET_OK;
 fail:
   if (publisher->impl) {
@@ -165,6 +169,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
   if (!rcl_node_is_valid(node, NULL)) {
     return RCL_RET_NODE_INVALID;
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing publisher")
   if (publisher->impl) {
     rcl_allocator_t allocator = publisher->impl->options.allocator;
     rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node);
@@ -179,6 +184,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
     }
     allocator.deallocate(publisher->impl, allocator.state);
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher finalized")
   return result;
 }
 
@@ -196,6 +202,7 @@ rcl_publisher_get_default_options()
 rcl_ret_t
 rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher publishing message")
   if (!rcl_publisher_is_valid(publisher)) {
     return RCL_RET_PUBLISHER_INVALID;
   }

rcl/src/rcl/rcl.c

@@ -23,6 +23,7 @@ extern "C"
 
 #include "./stdatomic_helper.h"
 #include "rcl/error_handling.h"
+#include "rcutils/logging_macros.h"
 #include "rmw/error_handling.h"
 
 static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false);
@@ -116,6 +117,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
 rcl_ret_t
 rcl_shutdown()
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Shutting down")
   if (!rcl_ok()) {
     // must use default allocator here because __rcl_allocator may not be set yet
     RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init", rcl_get_default_allocator());

rcl/src/rcl/rmw_implementation_identifier_check.c

@@ -57,7 +57,7 @@ INITIALIZER(initialize) {
   rcl_ret_t ret = rcl_impl_getenv("RCL_ASSERT_RMW_ID_MATCHES", &expected);
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
-      "rcl",
+      ROS_PACKAGE_NAME,
       "Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s",
       rcl_get_error_string_safe()
     )
@@ -66,7 +66,7 @@ INITIALIZER(initialize) {
   // If the environment variable is set, and it does not match, print a warning and exit.
   if (strlen(expected) > 0 && strcmp(rmw_get_implementation_identifier(), expected) != 0) {
     RCUTILS_LOG_ERROR_NAMED(
-      "rcl",
+      ROS_PACKAGE_NAME,
       "Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.",
       expected,
       rmw_get_implementation_identifier(),

rcl/src/rcl/service.c

@@ -64,6 +64,8 @@ rcl_service_init(
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Initializing service for service name '%s'", service_name)
   if (service->impl) {
     RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized", *allocator);
     return RCL_RET_ALREADY_INIT;
@@ -84,7 +86,7 @@ rcl_service_init(
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
-        "rcl",
+        ROS_PACKAGE_NAME,
         "failed to fini string_map (%d) during error handling: %s",
         rcutils_ret,
         rcutils_get_error_string_safe())
@@ -117,6 +119,7 @@ rcl_service_init(
       return RCL_RET_ERROR;
     }
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name)
   // Validate the expanded service name.
   int validation_result;
   rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL);
@@ -136,7 +139,7 @@ rcl_service_init(
 
   if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
     RCUTILS_LOG_WARN_NAMED(
-      "rcl",
+      ROS_PACKAGE_NAME,
       "Warning: Setting QoS durability to 'transient local' for service servers "
       "can cause them to receive requests from clients that have since terminated.")
   }
@@ -155,6 +158,7 @@ rcl_service_init(
   }
   // options
   service->impl->options = *options;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized")
   return RCL_RET_OK;
 fail:
   if (service->impl) {
@@ -166,6 +170,7 @@ rcl_service_init(
 rcl_ret_t
 rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing service")
   rcl_ret_t result = RCL_RET_OK;
   RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@@ -185,6 +190,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
     }
     allocator.deallocate(service->impl, allocator.state);
   }
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized")
   return result;
 }
 
@@ -239,6 +245,7 @@ rcl_take_request(
   rmw_request_id_t * request_header,
   void * ros_request)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service server taking service request")
   const rcl_service_options_t * options = rcl_service_get_options(service);
   RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT, options->allocator);
@@ -250,6 +257,8 @@ rcl_take_request(
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Service take request succeeded: %s", taken ? "true" : "false")
   if (!taken) {
     return RCL_RET_SERVICE_TAKE_FAILED;
   }
@@ -262,6 +271,7 @@ rcl_send_response(
   rmw_request_id_t * request_header,
   void * ros_response)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response")
   const rcl_service_options_t * options = rcl_service_get_options(service);
   RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator);
   RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT, options->allocator);
@@ -272,7 +282,6 @@ rcl_send_response(
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
-
   return RCL_RET_OK;
 }
 

rcl/src/rcl/stdatomic_helper/win32/stdatomic.h

@@ -209,7 +209,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_compare_exchange_strong") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_compare_exchange_strong") \
         exit(-1); \
         break; \
     } \
@@ -238,7 +238,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_exchange_strong") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_exchange_strong") \
         exit(-1); \
         break; \
     } \
@@ -264,7 +264,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_fetch_add") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_add") \
         exit(-1); \
         break; \
     } \
@@ -290,7 +290,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_fetch_and") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_and") \
         exit(-1); \
         break; \
     } \
@@ -316,7 +316,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_fetch_or") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_or") \
         exit(-1); \
         break; \
     } \
@@ -345,7 +345,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_fetch_xor") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_xor") \
         exit(-1); \
         break; \
     } \
@@ -371,7 +371,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
         break; \
       default: \
         RCUTILS_LOG_ERROR_NAMED( \
-          "rcl", "Unsupported integer type in atomic_load") \
+          ROS_PACKAGE_NAME, "Unsupported integer type in atomic_load") \
         exit(-1); \
         break; \
     } \