From a210c962d0d6db1c72dbc89c948848c2641d2ce8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 13 Jan 2020 12:56:15 -0300 Subject: [PATCH] Address PR comments Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/listener_thread.cpp | 40 +++++----- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 2 +- rmw_fastrtps_cpp/src/subscription.cpp | 2 +- .../src/listener_thread.cpp | 77 ++++++++----------- .../src/rmw_subscription.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 2 +- .../custom_subscriber_info.hpp | 2 +- rmw_fastrtps_shared_cpp/src/demangle.cpp | 6 ++ rmw_fastrtps_shared_cpp/src/demangle.hpp | 5 ++ .../src/rmw_node_info_and_types.cpp | 5 +- .../src/rmw_subscription.cpp | 2 +- .../src/rmw_topic_names_and_types.cpp | 5 +- 12 files changed, 72 insertions(+), 78 deletions(-) diff --git a/rmw_fastrtps_cpp/src/listener_thread.cpp b/rmw_fastrtps_cpp/src/listener_thread.cpp index 8dd13bd41..9347ea540 100644 --- a/rmw_fastrtps_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_cpp/src/listener_thread.cpp @@ -17,7 +17,7 @@ #include #include -#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 index 644fed4a1..5b559a451 100644 --- 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 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 index cd3624e90..8dbf50b73 100644 --- 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 index 23193845f..f8c366fd5 100644 --- a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp @@ -17,7 +17,7 @@ #include #include -#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(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(common_context->gid.data), reinterpret_cast(&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 index 33bede8db..47d599f59 100644 --- 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 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 index 5ae557171..af97b60a7 100644 --- 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 index 2ed9fede0..4bbf24ee3 100644 --- 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 index d7349eaec..4f50b84fc 100644 --- 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 index 1a1be5320..02a3e005a 100644 --- 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 index 15199d046..38a932f60 100644 --- 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(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 index 735c59f3b..a92d6a266 100644 --- 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 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 index ffa17be78..478759ad6 100644 --- 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(node->context->impl->common);