Split clean up code & assert preconditions.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rmw_cyclonedds_cpp/src/rmw_node.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cassert>
+#include <cstring>
 #include <mutex>
 #include <unordered_map>
 #include <unordered_set>
@@ -1140,11 +1142,10 @@ extern "C" rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t
     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 = eclipse_cyclonedds_identifier;
@@ -1154,12 +1155,15 @@ extern "C" rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t
     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;});
 
   if ((ret = rmw_init_options_copy(options, &context->options)) != RMW_RET_OK) {
     return ret;
   }
 
-  cleanup.cancel();
+  cleanup_impl.cancel();
+  restore_context.cancel();
   return RMW_RET_OK;
 }