diff --git a/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp b/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp index 0344c15d3..d4bb1a248 100644 --- a/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp +++ b/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -22,8 +23,15 @@ extern "C" { rmw_guard_condition_t * -rmw_create_guard_condition() +rmw_create_guard_condition(rmw_context_t * context) { + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored + return NULL); return rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( eprosima_fastrtps_identifier); } diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index fe9b50da0..e8d9e5948 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -12,13 +12,89 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_fastrtps_cpp/identifier.hpp" + extern "C" { rmw_ret_t -rmw_init() +rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + init_options->instance_id = 0; + init_options->implementation_identifier = eprosima_fastrtps_identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + *dst = *src; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_fini(rmw_init_options_t * init_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&(init_options->allocator), return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + *init_options = rmw_get_zero_initialized_init_options(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init(const rmw_init_options_t * options, rmw_context_t * context) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + options, + options->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + context->instance_id = options->instance_id; + context->implementation_identifier = eprosima_fastrtps_identifier; + context->impl = nullptr; + 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_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + // context impl is explicitly supposed to be nullptr for now, see rmw_init's code + // RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); + *context = rmw_get_zero_initialized_context(); return RMW_RET_OK; } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index 022db9a9a..8e37ed129 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -22,6 +22,7 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -32,11 +33,19 @@ extern "C" { rmw_node_t * rmw_create_node( + rmw_context_t * context, const char * name, const char * namespace_, size_t domain_id, const rmw_node_security_options_t * security_options) { + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored + return NULL); return rmw_fastrtps_shared_cpp::__rmw_create_node( eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp index 202ecc618..a7420fe21 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -22,8 +23,15 @@ extern "C" { rmw_guard_condition_t * -rmw_create_guard_condition() +rmw_create_guard_condition(rmw_context_t * context) { + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored + return NULL); return rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( eprosima_fastrtps_identifier); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index fe9b50da0..f5ccc3f1f 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -12,13 +12,89 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + extern "C" { rmw_ret_t -rmw_init() +rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + init_options->instance_id = 0; + init_options->implementation_identifier = eprosima_fastrtps_identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + *dst = *src; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_fini(rmw_init_options_t * init_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&(init_options->allocator), return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + *init_options = rmw_get_zero_initialized_init_options(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init(const rmw_init_options_t * options, rmw_context_t * context) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + options, + options->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + context->instance_id = options->instance_id; + context->implementation_identifier = eprosima_fastrtps_identifier; + context->impl = nullptr; + 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_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + // context impl is explicitly supposed to be nullptr for now, see rmw_init's code + // RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); + *context = rmw_get_zero_initialized_context(); return RMW_RET_OK; } } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 77d73c191..9c740eb63 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -22,6 +22,7 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -32,11 +33,19 @@ extern "C" { rmw_node_t * rmw_create_node( + rmw_context_t * context, const char * name, const char * namespace_, size_t domain_id, const rmw_node_security_options_t * security_options) { + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored + return NULL); return rmw_fastrtps_shared_cpp::__rmw_create_node( eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options); }