Address PR comments

Signed-off-by: Ivan Santiago Paunovic <[email protected]>
ros2 · Jan 13, 2020 · a210c96 · a210c96
1 parent 5428599
commit a210c96
Show file tree

Hide file tree

Showing 12 changed files with 72 additions and 78 deletions.
diff --git a/rmw_fastrtps_cpp/src/listener_thread.cpp b/rmw_fastrtps_cpp/src/listener_thread.cpp
@@ -17,7 +17,7 @@
 #include <cstring>
 #include <thread>
 
-#include "rcutils/logging_macros.h"
+#include "rcutils/macros.h"
 
 #include "rmw/allocators.h"
 #include "rmw/error_handling.h"
@@ -37,8 +37,6 @@
 
 using rmw_dds_common::operator<<;
 
-static const char log_tag[] = "rmw_dds_common";
-
 static
 void
 node_listener(rmw_context_t * context);
@@ -64,7 +62,10 @@ rmw_fastrtps_cpp::run_listener_thread(rmw_context_t * context)
   common_context->thread_is_running.store(false);
   if (common_context->listener_thread_gc) {
     if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) {
-      RCUTILS_LOG_ERROR_NAMED(log_tag, "Failed to destroy guard condition");
+      fprintf(
+        stderr,
+        RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":"
+        RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition");
     }
   }
   return RMW_RET_ERROR;
@@ -95,13 +96,15 @@ rmw_fastrtps_cpp::join_listener_thread(rmw_context_t * context)
   return RMW_RET_OK;
 }
 
-static
-void
-terminate(const char * error_message)
-{
-  RCUTILS_LOG_ERROR_NAMED(log_tag, "%s, terminating ...", error_message);
-  std::terminate();
-}
+#define TERMINATE(msg) \
+  do { \
+    fprintf( \
+      stderr, \
+      RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \
+      RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": %s, terminating ...", \
+      rmw_get_error_string().str); \
+    std::terminate(); \
+  } while (0)
 
 void
 node_listener(rmw_context_t * context)
@@ -124,7 +127,7 @@ node_listener(rmw_context_t * context)
     // number of conditions of a subscription is 2
     rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2);
     if (nullptr == wait_set) {
-      terminate("failed to create wait set");
+      TERMINATE("failed to create wait set");
     }
     if (RMW_RET_OK != rmw_wait(
         &subscriptions,
@@ -135,7 +138,7 @@ node_listener(rmw_context_t * context)
         wait_set,
         nullptr))
     {
-      terminate("rmw_wait failed");
+      TERMINATE("rmw_wait failed");
     }
     if (subscriptions_buffer[0]) {
       rmw_dds_common::msg::ParticipantEntitiesInfo msg;
@@ -146,7 +149,7 @@ node_listener(rmw_context_t * context)
           &taken,
           nullptr))
       {
-        terminate("rmw_take failed");
+        TERMINATE("rmw_take failed");
       }
       if (taken) {
         if (std::memcmp(
@@ -158,17 +161,10 @@ node_listener(rmw_context_t * context)
           continue;
         }
         common_context->graph_cache.update_participant_entities(msg);
-        if (rcutils_logging_logger_is_enabled_for("rmw_dds_common",
-          RCUTILS_LOG_SEVERITY_DEBUG))
-        {
-          std::ostringstream ss;
-          ss << common_context->graph_cache;
-          RCUTILS_LOG_DEBUG_NAMED(log_tag, "%s", ss.str().c_str());
-        }
       }
     }
     if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) {
-      terminate("failed to destroy wait set");
+      TERMINATE("failed to destroy wait set");
     }
   }
 }
diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp
@@ -92,7 +92,7 @@ rmw_create_subscription(
     std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
     rmw_dds_common::msg::ParticipantEntitiesInfo msg =
       common_context->graph_cache.associate_reader(
-      info->subscription_gid, common_context->gid, node->name, node->namespace_);
+      info->subscription_gid_, common_context->gid, node->name, node->namespace_);
     rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
       eprosima_fastrtps_identifier,
       common_context->pub,

diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp
@@ -138,7 +138,7 @@ create_subscription(
     RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber");
     goto fail;
   }
-  info->subscription_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+  info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid(
     eprosima_fastrtps_identifier, info->subscriber_->getGuid());
   rmw_subscription = rmw_subscription_allocate();
   if (!rmw_subscription) {

diff --git a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp b/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp
@@ -17,7 +17,7 @@
 #include <cstring>
 #include <thread>
 
-#include "rcutils/logging_macros.h"
+#include "rcutils/macros.h"
 
 #include "rmw/allocators.h"
 #include "rmw/error_handling.h"
@@ -37,8 +37,6 @@
 
 using rmw_dds_common::operator<<;
 
-static const char log_tag[] = "rmw_dds_common";
-
 static
 void
 node_listener(rmw_context_t * context);
@@ -49,31 +47,28 @@ rmw_fastrtps_dynamic_cpp::run_listener_thread(rmw_context_t * context)
   auto common_context = static_cast<rmw_dds_common::Context *>(context->impl->common);
   common_context->thread_is_running.store(true);
   common_context->listener_thread_gc = rmw_create_guard_condition(context);
-  auto clean = [ = ]() {
-      common_context->thread_is_running.store(false);
-      if (common_context->listener_thread_gc) {
-        if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) {
-          RCUTILS_LOG_ERROR_NAMED(log_tag, "Failed to destroy guard condition");
-        }
-      }
-    };
-  if (!common_context->listener_thread_gc) {
+  if (common_context->listener_thread_gc) {
+    try {
+      common_context->listener_thread = std::thread(node_listener, context);
+      return RMW_RET_OK;
+    } catch (const std::exception & exc) {
+      RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what());
+    } catch (...) {
+      RMW_SET_ERROR_MSG("Failed to create std::thread");
+    }
+  } else {
     RMW_SET_ERROR_MSG("Failed to create guard condition");
-    clean();
-    return RMW_RET_ERROR;
   }
-  try {
-    common_context->listener_thread = std::thread(node_listener, context);
-  } catch (const std::exception & exc) {
-    RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what());
-    clean();
-    return RMW_RET_ERROR;
-  } catch (...) {
-    RMW_SET_ERROR_MSG("Failed to create std::thread");
-    clean();
-    return RMW_RET_ERROR;
+  common_context->thread_is_running.store(false);
+  if (common_context->listener_thread_gc) {
+    if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) {
+      fprintf(
+        stderr,
+        RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":"
+        RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition");
+    }
   }
-  return RMW_RET_OK;
+  return RMW_RET_ERROR;
 }
 
 rmw_ret_t
@@ -101,13 +96,15 @@ rmw_fastrtps_dynamic_cpp::join_listener_thread(rmw_context_t * context)
   return RMW_RET_OK;
 }
 
-static
-void
-terminate(const char * error_message)
-{
-  RCUTILS_LOG_ERROR_NAMED(log_tag, "%s, terminating ...", error_message);
-  std::terminate();
-}
+#define TERMINATE(msg) \
+  do { \
+    fprintf( \
+      stderr, \
+      RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \
+      RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": %s, terminating ...", \
+      rmw_get_error_string().str); \
+    std::terminate(); \
+  } while (0)
 
 void
 node_listener(rmw_context_t * context)
@@ -130,7 +127,7 @@ node_listener(rmw_context_t * context)
     // number of conditions of a subscription is 2
     rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2);
     if (nullptr == wait_set) {
-      terminate("failed to create wait set");
+      TERMINATE("failed to create wait set");
     }
     if (RMW_RET_OK != rmw_wait(
         &subscriptions,
@@ -141,7 +138,7 @@ node_listener(rmw_context_t * context)
         wait_set,
         nullptr))
     {
-      terminate("rmw_wait failed");
+      TERMINATE("rmw_wait failed");
     }
     if (subscriptions_buffer[0]) {
       rmw_dds_common::msg::ParticipantEntitiesInfo msg;
@@ -152,10 +149,9 @@ node_listener(rmw_context_t * context)
           &taken,
           nullptr))
       {
-        terminate("rmw_take failed");
+        TERMINATE("rmw_take failed");
       }
       if (taken) {
-        // TODO(ivanpauno): Should the program be terminated if taken is false?
         if (std::memcmp(
             reinterpret_cast<char *>(common_context->gid.data),
             reinterpret_cast<char *>(&msg.gid.data),
@@ -165,17 +161,10 @@ node_listener(rmw_context_t * context)
           continue;
         }
         common_context->graph_cache.update_participant_entities(msg);
-        if (rcutils_logging_logger_is_enabled_for("rmw_dds_common",
-          RCUTILS_LOG_SEVERITY_DEBUG))
-        {
-          std::ostringstream ss;
-          ss << common_context->graph_cache;
-          RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "%s", ss.str().c_str());
-        }
       }
     }
     if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) {
-      terminate("failed to destroy wait set");
+      TERMINATE("failed to destroy wait set");
     }
   }
 }
diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp
@@ -98,7 +98,7 @@ rmw_create_subscription(
     std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
     rmw_dds_common::msg::ParticipantEntitiesInfo msg =
       common_context->graph_cache.associate_reader(
-      info->subscription_gid, common_context->gid, node->name, node->namespace_);
+      info->subscription_gid_, common_context->gid, node->name, node->namespace_);
     rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
       eprosima_fastrtps_identifier,
       common_context->pub,

diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp
@@ -140,7 +140,7 @@ create_subscription(
     RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber");
     goto fail;
   }
-  info->subscription_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+  info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid(
     eprosima_fastrtps_identifier, info->subscriber_->getGuid());
   rmw_subscription = rmw_subscription_allocate();
   if (!rmw_subscription) {

diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp
@@ -41,7 +41,7 @@ struct CustomSubscriberInfo : public CustomEventInfo
   eprosima::fastrtps::Subscriber * subscriber_;
   SubListener * listener_;
   rmw_fastrtps_shared_cpp::TypeSupport * type_support_;
-  rmw_gid_t subscription_gid;
+  rmw_gid_t subscription_gid_;
   const char * typesupport_identifier_;
 
   RMW_FASTRTPS_SHARED_CPP_PUBLIC

diff --git a/rmw_fastrtps_shared_cpp/src/demangle.cpp b/rmw_fastrtps_shared_cpp/src/demangle.cpp
@@ -157,3 +157,9 @@ _demangle_service_type_only(const std::string & dds_type_name)
   std::string type_name = dds_type_name.substr(start, suffix_position - start);
   return type_namespace + type_name;
 }
+
+std::string
+_identity_demangle(const std::string & name)
+{
+  return name;
+}
diff --git a/rmw_fastrtps_shared_cpp/src/demangle.hpp b/rmw_fastrtps_shared_cpp/src/demangle.hpp
@@ -46,6 +46,11 @@ _demangle_service_reply_from_topic(const std::string & topic_name);
 std::string
 _demangle_service_type_only(const std::string & dds_type_name);
 
+/// Used when ros names are not mangled.
+std::string
+_identity_demangle(const std::string & name);
+
+
 using DemangleFunction = std::string (*)(const std::string &);
 using MangleFunction = DemangleFunction;
 

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp
@@ -122,10 +122,9 @@ __rmw_get_topic_names_and_types_by_node(
   }
   auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
 
-  DemangleFunction no_op{[](const std::string & x) {return x;}};
   if (no_demangle) {
-    demangle_topic = no_op;
-    demangle_type = no_op;
+    demangle_topic = _identity_demangle;
+    demangle_type = _identity_demangle;
   }
 
   return get_names_and_types_by_node(

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp
@@ -56,7 +56,7 @@ __rmw_destroy_subscription(
     std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
     rmw_dds_common::msg::ParticipantEntitiesInfo msg =
       common_context->graph_cache.dissociate_reader(
-      info->subscription_gid, common_context->gid, node->name, node->namespace_);
+      info->subscription_gid_, common_context->gid, node->name, node->namespace_);
     rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
       identifier,
       common_context->pub,

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp
@@ -61,11 +61,10 @@ __rmw_get_topic_names_and_types(
 
   DemangleFunction demangle_topic = _demangle_ros_topic_from_topic;
   DemangleFunction demangle_type = _demangle_if_ros_type;
-  DemangleFunction no_op{[](const std::string & x) {return x;}};
 
   if (no_demangle) {
-    demangle_topic = no_op;
-    demangle_type = no_op;
+    demangle_topic = _identity_demangle;
+    demangle_type = _identity_demangle;
   }
   auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);