diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index e76489823..0ab4a609c 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -162,6 +162,14 @@ rmw_fastrtps_cpp::create_publisher( RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); return nullptr; } + auto cleanup_publisher = rcpputils::make_scope_exit( + [info]() { + if (!Domain::removePublisher(info->publisher_)) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to remove publisher after '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->publisher_->getGuid()); @@ -171,7 +179,7 @@ rmw_fastrtps_cpp::create_publisher( RMW_SET_ERROR_MSG("failed to allocate publisher"); return nullptr; } - auto cleanup_publisher = rcpputils::make_scope_exit( + auto cleanup_rmw_publisher = rcpputils::make_scope_exit( [rmw_publisher]() { rmw_free(const_cast(rmw_publisher->topic_name)); rmw_publisher_free(rmw_publisher); @@ -189,6 +197,7 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher->options = *publisher_options; + cleanup_rmw_publisher.cancel(); cleanup_publisher.cancel(); cleanup_info.cancel(); return rmw_publisher; diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 8a602b613..68328611e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -177,6 +177,14 @@ rmw_fastrtps_dynamic_cpp::create_publisher( RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); return nullptr; } + auto cleanup_publisher = rcpputils::make_scope_exit( + [info]() { + if (!Domain::removePublisher(info->publisher_)) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to remove publisher after '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->publisher_->getGuid()); @@ -186,7 +194,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( RMW_SET_ERROR_MSG("failed to allocate publisher"); return nullptr; } - auto cleanup_publisher = rcpputils::make_scope_exit( + auto cleanup_rmw_publisher = rcpputils::make_scope_exit( [rmw_publisher]() { rmw_free(const_cast(rmw_publisher->topic_name)); rmw_publisher_free(rmw_publisher); @@ -205,6 +213,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher->options = *publisher_options; + cleanup_rmw_publisher.cancel(); cleanup_publisher.cancel(); cleanup_info.cancel(); return_type_support.cancel();