Add debug logging

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());
@@ -272,6 +277,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 request 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.

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);

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());
@@ -250,6 +255,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;
   }

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; \
     } \

rcl/src/rcl/subscription.c

@@ -63,6 +63,8 @@ rcl_subscription_init(
   RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, *allocator);
   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 subscription for topic name '%s'", topic_name)
   if (subscription->impl) {
     RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized", *allocator);
     return RCL_RET_ALREADY_INIT;
@@ -83,7 +85,7 @@ rcl_subscription_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())
@@ -116,6 +118,7 @@ rcl_subscription_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_subscription_init(
   }
   // options
   subscription->impl->options = *options;
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized")
   return RCL_RET_OK;
 fail:
   if (subscription->impl) {
@@ -159,6 +163,7 @@ rcl_subscription_init(
 rcl_ret_t
 rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
 {
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing subscription")
   rcl_ret_t result = RCL_RET_OK;
   RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@@ -224,6 +229,8 @@ rcl_take(
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Subscription take request succeeded: %s", taken ? "true" : "false")
   if (!taken) {
     return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
   }