From f4f381077b9a2ae2ff1aa7727caf931d3fcbdb74 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 22 Jun 2020 14:20:20 -0300 Subject: [PATCH 1/4] Ensure compliant init/shutdown API implementation. Signed-off-by: Michel Hidalgo --- rmw_fastrtps_cpp/src/rmw_init.cpp | 61 +++++++++++++---------- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 60 ++++++++++++---------- 2 files changed, 70 insertions(+), 51 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 9fda70589..454d4b97f 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -18,7 +18,9 @@ #include "rcutils/strdup.h" #include "rcutils/types.h" +#include "rmw/error_handling.h" #include "rmw/init.h" +#include "rmw/impl/cpp/atexit.hpp" #include "rmw/impl/cpp/macros.hpp" #include "rmw/init_options.h" #include "rmw/publisher_options.h" @@ -68,55 +70,57 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { - std::unique_ptr clean_when_fail( - context, - [](rmw_context_t * context) - { - *context = rmw_get_zero_initialized_context(); - }); - rmw_ret_t ret = RMW_RET_OK; - - RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + options->implementation_identifier, + "expected initialized init options", + return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( options, options->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != context->implementation_identifier) { + RMW_SET_ERROR_MSG("expected a zero-initialized context"); + return RMW_RET_INVALID_ARGUMENT; + } + + rmw::impl::cpp::atexit cleanup{[context]() { + delete context->impl; + *context = rmw_get_zero_initialized_context(); + }}; + context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; - - std::unique_ptr context_impl(new (std::nothrow) rmw_context_impl_t()); - if (!context_impl) { + context->impl = new (std::nothrow) rmw_context_impl_t(); + if (nullptr == context->impl) { RMW_SET_ERROR_MSG("failed to allocate context impl"); return RMW_RET_BAD_ALLOC; } + context->impl->is_shutdown = false; context->options = rmw_get_zero_initialized_init_options(); - ret = rmw_init_options_copy(options, &context->options); + rmw_ret_t ret = rmw_init_options_copy(options, &context->options); if (RMW_RET_OK != ret) { - if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { - RMW_SAFE_FWRITE_TO_STDERR( - "'rmw_init_options_fini' failed while being executed due to '" - RCUTILS_STRINGIFY(__function__) "' failing.\n"); - } return ret; } - context_impl->is_shutdown = false; - context->impl = context_impl.release(); - clean_when_fail.release(); + cleanup.cancel(); return RMW_RET_OK; } rmw_ret_t rmw_shutdown(rmw_context_t * context) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); context->impl->is_shutdown = true; return RMW_RET_OK; } @@ -124,7 +128,11 @@ rmw_shutdown(rmw_context_t * context) rmw_ret_t rmw_context_fini(rmw_context_t * context) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, @@ -134,8 +142,9 @@ rmw_context_fini(rmw_context_t * context) RCUTILS_SET_ERROR_MSG("context has not been shutdown"); return RMW_RET_INVALID_ARGUMENT; } + rmw_ret_t ret = rmw_init_options_fini(&context->options); delete context->impl; *context = rmw_get_zero_initialized_context(); - return RMW_RET_OK; + return ret; } } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index aedf8974d..34aacf5ff 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -18,6 +18,8 @@ #include "rcutils/strdup.h" #include "rcutils/types.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/atexit.hpp" #include "rmw/impl/cpp/macros.hpp" #include "rmw/init.h" #include "rmw/init_options.h" @@ -68,54 +70,57 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { - std::unique_ptr clean_when_fail( - context, - [](rmw_context_t * context) - { - *context = rmw_get_zero_initialized_context(); - }); - rmw_ret_t ret = RMW_RET_OK; - - RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + options->implementation_identifier, + "expected initialized init options", + return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( options, options->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != context->implementation_identifier) { + RMW_SET_ERROR_MSG("expected a zero-initialized context"); + return RMW_RET_INVALID_ARGUMENT; + } + + rmw::impl::cpp::atexit cleanup{[context]() { + delete context->impl; + *context = rmw_get_zero_initialized_context(); + }}; + context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; - - std::unique_ptr context_impl(new (std::nothrow) rmw_context_impl_t()); - if (!context_impl) { + context->impl = new (std::nothrow) rmw_context_impl_t(); + if (nullptr == context->impl) { RMW_SET_ERROR_MSG("failed to allocate context impl"); return RMW_RET_BAD_ALLOC; } + context->impl->is_shutdown = false; context->options = rmw_get_zero_initialized_init_options(); - ret = rmw_init_options_copy(options, &context->options); + rmw_ret_t ret = rmw_init_options_copy(options, &context->options); if (RMW_RET_OK != ret) { - if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { - RMW_SAFE_FWRITE_TO_STDERR( - "'rmw_init_options_fini' failed while being executed due to '" - RCUTILS_STRINGIFY(__function__) "' failing.\n"); - } return ret; } - context->impl = context_impl.release(); - clean_when_fail.release(); + cleanup.cancel(); return RMW_RET_OK; } rmw_ret_t rmw_shutdown(rmw_context_t * context) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); context->impl->is_shutdown = true; return RMW_RET_OK; } @@ -123,7 +128,11 @@ rmw_shutdown(rmw_context_t * context) rmw_ret_t rmw_context_fini(rmw_context_t * context) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, @@ -133,8 +142,9 @@ rmw_context_fini(rmw_context_t * context) RCUTILS_SET_ERROR_MSG("context has not been shutdown"); return RMW_RET_INVALID_ARGUMENT; } + rmw_ret_t ret = rmw_init_options_fini(&context->options); delete context->impl; *context = rmw_get_zero_initialized_context(); - return RMW_RET_OK; + return ret; } } // extern "C" From 4e6c5e1fa46cf72ef17cad509f9628f5bc585144 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 25 Jun 2020 19:08:07 -0300 Subject: [PATCH 2/4] Use rcpputils::scope_exit(). Signed-off-by: Michel Hidalgo --- rmw_fastrtps_cpp/CMakeLists.txt | 2 ++ rmw_fastrtps_cpp/package.xml | 2 ++ rmw_fastrtps_cpp/src/rmw_init.cpp | 7 ++++--- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 7 ++++--- 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 963b199a4..0c5efb02c 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -31,6 +31,7 @@ endif() find_package(ament_cmake_ros REQUIRED) +find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) @@ -93,6 +94,7 @@ target_link_libraries(rmw_fastrtps_cpp # specific order: dependents before dependencies ament_target_dependencies(rmw_fastrtps_cpp + "rcpputils" "rcutils" "rosidl_typesupport_fastrtps_c" "rosidl_typesupport_fastrtps_cpp" diff --git a/rmw_fastrtps_cpp/package.xml b/rmw_fastrtps_cpp/package.xml index 776e4afe6..b216426f4 100644 --- a/rmw_fastrtps_cpp/package.xml +++ b/rmw_fastrtps_cpp/package.xml @@ -18,6 +18,7 @@ fastcdr fastrtps fastrtps_cmake_module + rcpputils rcutils rmw rmw_dds_common @@ -39,6 +40,7 @@ rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp + rcpputils rcutils rmw rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 454d4b97f..e25d59508 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -20,12 +20,13 @@ #include "rmw/error_handling.h" #include "rmw/init.h" -#include "rmw/impl/cpp/atexit.hpp" #include "rmw/impl/cpp/macros.hpp" #include "rmw/init_options.h" #include "rmw/publisher_options.h" #include "rmw/rmw.h" +#include "rcpputils/scope_exit.hpp" + #include "rmw_dds_common/context.hpp" #include "rmw_dds_common/msg/participant_entities_info.hpp" @@ -86,10 +87,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - rmw::impl::cpp::atexit cleanup{[context]() { + auto cleanup = rcpputils::make_scope_exit([context]() { delete context->impl; *context = rmw_get_zero_initialized_context(); - }}; + }); context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index 34aacf5ff..29f807e64 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -19,13 +19,14 @@ #include "rcutils/types.h" #include "rmw/error_handling.h" -#include "rmw/impl/cpp/atexit.hpp" #include "rmw/impl/cpp/macros.hpp" #include "rmw/init.h" #include "rmw/init_options.h" #include "rmw/publisher_options.h" #include "rmw/rmw.h" +#include "rcpputils/scope_exit.hpp" + #include "rmw_dds_common/context.hpp" #include "rmw_dds_common/msg/participant_entities_info.hpp" @@ -86,10 +87,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - rmw::impl::cpp::atexit cleanup{[context]() { + auto cleanup = rcpputils::make_scope_exit([context]() { delete context->impl; *context = rmw_get_zero_initialized_context(); - }}; + }); context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; From 813ee129c2741349a296aad9b0c10b466a0d1a4b Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 26 Jun 2020 18:35:37 -0300 Subject: [PATCH 3/4] Please uncrustify. Signed-off-by: Michel Hidalgo --- rmw_fastrtps_cpp/src/rmw_init.cpp | 3 ++- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index e25d59508..0238846d1 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -87,7 +87,8 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - auto cleanup = rcpputils::make_scope_exit([context]() { + auto cleanup = rcpputils::make_scope_exit( + [context]() { delete context->impl; *context = rmw_get_zero_initialized_context(); }); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index 29f807e64..c04cbb4ab 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -87,7 +87,8 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - auto cleanup = rcpputils::make_scope_exit([context]() { + auto cleanup = rcpputils::make_scope_exit( + [context]() { delete context->impl; *context = rmw_get_zero_initialized_context(); }); From f91573ea85d4b2909a1793b9cab3510cb3bbdfca Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 29 Jun 2020 12:01:25 -0300 Subject: [PATCH 4/4] Split clean up code & assert preconditions. Signed-off-by: Michel Hidalgo --- rmw_fastrtps_cpp/src/rmw_init.cpp | 19 +++++++++++++------ rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 19 +++++++++++++------ 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 0238846d1..e621b5b6e 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -13,6 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include "rcutils/strdup.h" @@ -87,26 +89,31 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - auto cleanup = rcpputils::make_scope_exit( - [context]() { - delete context->impl; - *context = rmw_get_zero_initialized_context(); - }); + const rmw_context_t zero_context = rmw_get_zero_initialized_context(); + assert(0 == std::memcmp(context, &zero_context, sizeof(rmw_context_t))); + auto restore_context = rcpputils::make_scope_exit( + [context, &zero_context]() {*context = zero_context;}); context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; + context->impl = new (std::nothrow) rmw_context_impl_t(); if (nullptr == context->impl) { RMW_SET_ERROR_MSG("failed to allocate context impl"); return RMW_RET_BAD_ALLOC; } + auto cleanup_impl = rcpputils::make_scope_exit( + [context]() {delete context->impl;}); + context->impl->is_shutdown = false; context->options = rmw_get_zero_initialized_init_options(); rmw_ret_t ret = rmw_init_options_copy(options, &context->options); if (RMW_RET_OK != ret) { return ret; } - cleanup.cancel(); + + cleanup_impl.cancel(); + restore_context.cancel(); return RMW_RET_OK; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index c04cbb4ab..a5a35143d 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -13,6 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include "rcutils/strdup.h" @@ -87,26 +89,31 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - auto cleanup = rcpputils::make_scope_exit( - [context]() { - delete context->impl; - *context = rmw_get_zero_initialized_context(); - }); + const rmw_context_t zero_context = rmw_get_zero_initialized_context(); + assert(0 == std::memcmp(context, &zero_context, sizeof(rmw_context_t))); + auto restore_context = rcpputils::make_scope_exit( + [context, &zero_context]() {*context = zero_context;}); context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; + context->impl = new (std::nothrow) rmw_context_impl_t(); if (nullptr == context->impl) { RMW_SET_ERROR_MSG("failed to allocate context impl"); return RMW_RET_BAD_ALLOC; } + auto cleanup_impl = rcpputils::make_scope_exit( + [context]() {delete context->impl;}); + context->impl->is_shutdown = false; context->options = rmw_get_zero_initialized_init_options(); rmw_ret_t ret = rmw_init_options_copy(options, &context->options); if (RMW_RET_OK != ret) { return ret; } - cleanup.cancel(); + + cleanup_impl.cancel(); + restore_context.cancel(); return RMW_RET_OK; }