rmw_waitset_t *
create_waitset(const char * implementation_identifier, size_t max_conditions)
{
rmw_waitset_t * waitset = rmw_waitset_allocate();
ConnextWaitSetInfo * waitset_info = nullptr;
// From here onward, error results in unrolling in the goto fail block.
if (!waitset) {
RMW_SET_ERROR_MSG("failed to allocate waitset");
goto fail;
}
waitset->implementation_identifier = implementation_identifier;
waitset->data = rmw_allocate(sizeof(ConnextWaitSetInfo));
waitset_info = static_cast<ConnextWaitSetInfo *>(waitset->data);
if (!waitset_info) {
RMW_SET_ERROR_MSG("failed to allocate waitset");
goto fail;
}
waitset_info->waitset = static_cast<DDSWaitSet *>(rmw_allocate(sizeof(DDSWaitSet)));
if (!waitset_info->waitset) {
RMW_SET_ERROR_MSG("failed to allocate waitset");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
waitset_info->waitset, waitset_info->waitset, goto fail, DDSWaitSet)
// Now allocate storage for the ConditionSeq objects
waitset_info->active_conditions =
static_cast<DDSConditionSeq *>(rmw_allocate(sizeof(DDSConditionSeq)));
if (!waitset_info->active_conditions) {
RMW_SET_ERROR_MSG("failed to allocate active_conditions sequence");
goto fail;
}
waitset_info->attached_conditions =
static_cast<DDSConditionSeq *>(rmw_allocate(sizeof(DDSConditionSeq)));
if (!waitset_info->attached_conditions) {
RMW_SET_ERROR_MSG("failed to allocate attached_conditions sequence");
goto fail;
}
// If max_conditions is greater than zero, re-allocate both ConditionSeqs to max_conditions
if (max_conditions > 0) {
RMW_TRY_PLACEMENT_NEW(
waitset_info->active_conditions, waitset_info->active_conditions, goto fail,
DDSConditionSeq, static_cast<DDS_Long>(max_conditions))
RMW_TRY_PLACEMENT_NEW(
waitset_info->attached_conditions, waitset_info->attached_conditions, goto fail,
DDSConditionSeq,
static_cast<DDS_Long>(max_conditions))
} else {
void
rmw_set_error_state(const char * error_string, const char * file, size_t line_number)
{
rmw_error_state_t * old_error_state = __rmw_error_state;
#if RMW_REPORT_ERROR_HANDLING_ERRORS
const char * old_error_string = rmw_get_error_string_safe();
#endif
__rmw_error_state = (rmw_error_state_t *)rmw_allocate(sizeof(rmw_error_state_t));
if (!__rmw_error_state) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
// rmw_allocate failed, but fwrite might work?
SAFE_FWRITE_TO_STDERR(
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__)
"] failed to allocate memory for the error state struct\n");
#endif
return;
}
size_t error_string_length = strlen(error_string);
// the memory must be one byte bigger to store the NULL character
__rmw_error_state->message = (char *)malloc(error_string_length + 1);
if (!__rmw_error_state->message) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
// malloc failed, but fwrite might work?
SAFE_FWRITE_TO_STDERR(
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__)
"] failed to allocate memory for the error message in the error state struct\n");
#endif
rmw_reset_error(); // This will free any allocations done so far.
return;
}
// Cast the const away to set ->message initially.
#ifndef _WIN32
snprintf((char *)__rmw_error_state->message, error_string_length + 1, "%s", error_string);
#else
auto retcode = strcpy_s(
(char *)__rmw_error_state->message, error_string_length + 1, error_string);
if (retcode) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
SAFE_FWRITE_TO_STDERR(
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__)
"] failed to copy error message in the error state struct\n");
#endif
}
#endif
__rmw_error_state->file = file;
__rmw_error_state->line_number = line_number;
if (__rmw_error_is_set(old_error_state)) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
// Only warn of overwritting if the new error string is different from the old ones.
if (error_string != old_error_string && error_string != old_error_state->message) {
fprintf(
stderr,
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__) "] error string being overwritten: %s\n",
old_error_string);
}
#endif
__rmw_reset_error(&old_error_state);
}
__rmw_reset_error_string(&__rmw_error_string);
}
static void
format_error_string()
{
if (!__rmw_error_is_set(__rmw_error_state)) {
return;
}
size_t bytes_it_would_have_written = snprintf(
NULL, 0,
__error_format_string,
__rmw_error_state->message, __rmw_error_state->file, __rmw_error_state->line_number);
__rmw_error_string = (char *)rmw_allocate(bytes_it_would_have_written + 1);
if (!__rmw_error_string) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
// rmw_allocate failed, but fwrite might work?
SAFE_FWRITE_TO_STDERR(
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__)
"] failed to allocate memory for the error string\n");
#endif
return;
}
snprintf(
__rmw_error_string, bytes_it_would_have_written + 1,
__error_format_string,
__rmw_error_state->message, __rmw_error_state->file, __rmw_error_state->line_number);
// The Windows version of snprintf does not null terminate automatically in all cases.
__rmw_error_string[bytes_it_would_have_written] = '\0';
}
rmw_guard_condition_t *
create_guard_condition(const char * implementation_identifier)
{
rmw_guard_condition_t * guard_condition = rmw_guard_condition_allocate();
if (!guard_condition) {
RMW_SET_ERROR_MSG("failed to allocate guard condition");
return NULL;
}
// Allocate memory for the DDSGuardCondition object.
DDSGuardCondition * dds_guard_condition = nullptr;
void * buf = rmw_allocate(sizeof(DDSGuardCondition));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
// Use a placement new to construct the DDSGuardCondition in the preallocated buffer.
RMW_TRY_PLACEMENT_NEW(dds_guard_condition, buf, goto fail, DDSGuardCondition)
buf = nullptr; // Only free the dds_guard_condition pointer; don't need the buf pointer anymore.
guard_condition->implementation_identifier = implementation_identifier;
guard_condition->data = dds_guard_condition;
return guard_condition;
fail:
if (guard_condition) {
rmw_guard_condition_free(guard_condition);
}
if (buf) {
rmw_free(buf);
}
return NULL;
}
rmw_guard_condition_t *
rmw_guard_condition_allocate()
{
// Could be overridden with custom (maybe static) guard_condition
// struct allocator
return (rmw_guard_condition_t *)rmw_allocate(sizeof(rmw_guard_condition_t));
}
void
rmw_set_error_state(const char * error_string, const char * file, size_t line_number)
{
if (rmw_error_is_set()) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
fprintf(
stderr,
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__) "] error string being overwritten: %s\n",
rmw_get_error_string_safe());
#endif
rmw_reset_error();
}
__rmw_error_state = (rmw_error_state_t *)rmw_allocate(sizeof(rmw_error_state_t));
if (!__rmw_error_state) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
// rmw_allocate failed, but fwrite might work?
SAFE_FWRITE_TO_STDERR(
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__)
"] failed to allocate memory for the error state struct\n");
#endif
return;
}
__rmw_error_state->message = (char *)malloc(strlen(error_string));
if (!__rmw_error_state->message) {
#if RMW_REPORT_ERROR_HANDLING_ERRORS
// rmw_allocate failed, but fwrite might work?
SAFE_FWRITE_TO_STDERR(
"[rmw|error_handling.c:" RMW_STRINGIFY(__LINE__)
"] failed to allocate memory for the error message in the error state struct\n");
#endif
rmw_reset_error(); // This will free any allocations done so far.
return;
}
// Cast the const away to set ->message initially.
strcpy((char *)__rmw_error_state->message, error_string);
__rmw_error_state->file = file;
__rmw_error_state->line_number = line_number;
}
rmw_node_t *
create_node(const char * implementation_identifier, const char * name, size_t domain_id)
{
DDSDomainParticipantFactory * dpf_ = DDSDomainParticipantFactory::get_instance();
if (!dpf_) {
RMW_SET_ERROR_MSG("failed to get participant factory");
return NULL;
}
// use loopback interface to enable cross vendor communication
DDS_DomainParticipantQos participant_qos;
DDS_ReturnCode_t status = dpf_->get_default_participant_qos(participant_qos);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to get default participant qos");
return NULL;
}
// forces local traffic to be sent over loopback,
// even if a more efficient transport (such as shared memory) is installed
// (in which case traffic will be sent over both transports)
status = DDSPropertyQosPolicyHelper::add_property(
participant_qos.property,
"dds.transport.UDPv4.builtin.ignore_loopback_interface",
"0",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return NULL;
}
status = DDSPropertyQosPolicyHelper::add_property(
participant_qos.property,
"dds.transport.use_510_compatible_locator_kinds",
"1",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return NULL;
}
DDS_DomainId_t domain = static_cast<DDS_DomainId_t>(domain_id);
DDSDomainParticipant * participant = dpf_->create_participant(
domain, participant_qos, NULL,
DDS_STATUS_MASK_NONE);
if (!participant) {
RMW_SET_ERROR_MSG("failed to create participant");
return NULL;
}
rmw_node_t * node_handle = nullptr;
ConnextNodeInfo * node_info = nullptr;
rmw_guard_condition_t * graph_guard_condition = nullptr;
CustomPublisherListener * publisher_listener = nullptr;
CustomSubscriberListener * subscriber_listener = nullptr;
void * buf = nullptr;
DDSDataReader * data_reader = nullptr;
DDSPublicationBuiltinTopicDataDataReader * builtin_publication_datareader = nullptr;
DDSSubscriptionBuiltinTopicDataDataReader * builtin_subscription_datareader = nullptr;
DDSSubscriber * builtin_subscriber = participant->get_builtin_subscriber();
if (!builtin_subscriber) {
RMW_SET_ERROR_MSG("builtin subscriber handle is null");
goto fail;
}
// setup publisher listener
data_reader = builtin_subscriber->lookup_datareader(DDS_PUBLICATION_TOPIC_NAME);
builtin_publication_datareader =
static_cast<DDSPublicationBuiltinTopicDataDataReader *>(data_reader);
if (!builtin_publication_datareader) {
RMW_SET_ERROR_MSG("builtin publication datareader handle is null");
goto fail;
}
graph_guard_condition = create_guard_condition(implementation_identifier);
if (!graph_guard_condition) {
RMW_SET_ERROR_MSG("failed to create graph guard condition");
goto fail;
}
buf = rmw_allocate(sizeof(CustomPublisherListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
publisher_listener, buf, goto fail, CustomPublisherListener,
implementation_identifier, graph_guard_condition)
buf = nullptr;
builtin_publication_datareader->set_listener(publisher_listener, DDS_DATA_AVAILABLE_STATUS);
data_reader = builtin_subscriber->lookup_datareader(DDS_SUBSCRIPTION_TOPIC_NAME);
builtin_subscription_datareader =
static_cast<DDSSubscriptionBuiltinTopicDataDataReader *>(data_reader);
if (!builtin_subscription_datareader) {
RMW_SET_ERROR_MSG("builtin subscription datareader handle is null");
goto fail;
}
// setup subscriber listener
buf = rmw_allocate(sizeof(CustomSubscriberListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
subscriber_listener, buf, goto fail, CustomSubscriberListener,
implementation_identifier, graph_guard_condition)
buf = nullptr;
builtin_subscription_datareader->set_listener(subscriber_listener, DDS_DATA_AVAILABLE_STATUS);
node_handle = rmw_node_allocate();
if (!node_handle) {
RMW_SET_ERROR_MSG("failed to allocate memory for node handle");
goto fail;
}
node_handle->implementation_identifier = implementation_identifier;
node_handle->data = participant;
node_handle->name =
reinterpret_cast<const char *>(rmw_allocate(sizeof(char) * strlen(name) + 1));
if (!node_handle->name) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
memcpy(const_cast<char *>(node_handle->name), name, strlen(name) + 1);
buf = rmw_allocate(sizeof(ConnextNodeInfo));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(node_info, buf, goto fail, ConnextNodeInfo)
buf = nullptr;
node_info->participant = participant;
node_info->publisher_listener = publisher_listener;
node_info->subscriber_listener = subscriber_listener;
node_info->graph_guard_condition = graph_guard_condition;
node_handle->implementation_identifier = implementation_identifier;
node_handle->data = node_info;
return node_handle;
fail:
status = dpf_->delete_participant(participant);
if (status != DDS_RETCODE_OK) {
std::stringstream ss;
ss << "leaking participant while handling failure at " <<
__FILE__ << ":" << __LINE__;
(std::cerr << ss.str()).flush();
}
if (graph_guard_condition) {
rmw_ret_t ret = destroy_guard_condition(implementation_identifier, graph_guard_condition);
if (ret != RMW_RET_OK) {
std::stringstream ss;
ss << "failed to destroy guard condition while handling failure at " <<
__FILE__ << ":" << __LINE__;
(std::cerr << ss.str()).flush();
}
}
if (publisher_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
publisher_listener->~CustomPublisherListener(), CustomPublisherListener)
rmw_free(publisher_listener);
}
if (subscriber_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener)
rmw_free(subscriber_listener);
}
if (node_handle) {
if (node_handle->name) {
rmw_free(const_cast<char *>(node_handle->name));
}
rmw_free(node_handle);
}
if (node_info) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->~ConnextNodeInfo(), ConnextNodeInfo)
rmw_free(node_info);
}
if (buf) {
rmw_free(buf);
}
return NULL;
}
rmw_client_t *
rmw_create_client(
const rmw_node_t * node,
const rosidl_service_type_support_t * type_support,
const char * service_name,
const rmw_qos_profile_t * qos_profile)
{
if (!node) {
RMW_SET_ERROR_MSG("node handle is null");
return nullptr;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
node handle,
node->implementation_identifier, opensplice_cpp_identifier,
return nullptr)
if (!type_support) {
RMW_SET_ERROR_MSG("type support handle is null");
return nullptr;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
type support,
type_support->typesupport_identifier,
rosidl_typesupport_opensplice_cpp::typesupport_opensplice_identifier,
return nullptr)
if (!qos_profile) {
RMW_SET_ERROR_MSG("qos_profile is null");
return nullptr;
}
auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
if (!node_info) {
RMW_SET_ERROR_MSG("node info handle is null");
return NULL;
}
auto participant = static_cast<DDS::DomainParticipant *>(node_info->participant);
if (!participant) {
RMW_SET_ERROR_MSG("participant handle is null");
return NULL;
}
const service_type_support_callbacks_t * callbacks =
static_cast<const service_type_support_callbacks_t *>(type_support->data);
if (!callbacks) {
RMW_SET_ERROR_MSG("callbacks handle is null");
return NULL;
}
DDS::DataReaderQos datareader_qos;
DDS::DataWriterQos datawriter_qos;
// Past this point, a failure results in unrolling code in the goto fail block.
rmw_client_t * client = nullptr;
const char * error_string = nullptr;
DDS::DataReader * response_datareader = nullptr;
DDS::ReadCondition * read_condition = nullptr;
void * requester = nullptr;
OpenSpliceStaticClientInfo * client_info = nullptr;
// Begin initializing elements.
client = rmw_client_allocate();
if (!client) {
RMW_SET_ERROR_MSG("failed to allocate client");
goto fail;
}
if (!get_datareader_qos(nullptr, *qos_profile, datareader_qos)) {
goto fail;
}
if (!get_datawriter_qos(nullptr, *qos_profile, datawriter_qos)) {
goto fail;
}
error_string = callbacks->create_requester(
participant, service_name,
reinterpret_cast<void **>(&requester),
reinterpret_cast<void **>(&response_datareader),
reinterpret_cast<const void *>(&datareader_qos),
reinterpret_cast<const void *>(&datawriter_qos),
&rmw_allocate);
if (error_string) {
RMW_SET_ERROR_MSG((std::string("failed to create requester: ") + error_string).c_str());
goto fail;
}
if (!requester) {
RMW_SET_ERROR_MSG("failed to create requester: requester is null");
goto fail;
}
if (!response_datareader) {
RMW_SET_ERROR_MSG("failed to create requester: response_datareader is null");
goto fail;
}
read_condition = response_datareader->create_readcondition(
DDS::ANY_SAMPLE_STATE, DDS::ANY_VIEW_STATE, DDS::ANY_INSTANCE_STATE);
if (!read_condition) {
RMW_SET_ERROR_MSG("failed to create read condition");
goto fail;
}
client_info = static_cast<OpenSpliceStaticClientInfo *>(
rmw_allocate(sizeof(OpenSpliceStaticClientInfo)));
if (!client_info) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
client_info->requester_ = requester;
client_info->callbacks_ = callbacks;
client_info->response_datareader_ = response_datareader;
client_info->read_condition_ = read_condition;
client->implementation_identifier = opensplice_cpp_identifier;
client->data = client_info;
client->service_name = reinterpret_cast<const char *>(rmw_allocate(strlen(service_name) + 1));
if (!client->service_name) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
memcpy(const_cast<char *>(client->service_name), service_name, strlen(service_name) + 1);
return client;
fail:
if (response_datareader) {
if (read_condition) {
if (response_datareader->delete_readcondition(read_condition) != DDS::RETCODE_OK) {
fprintf(stderr, "leaking readcondition while handling failure\n");
}
}
}
if (requester) {
const char * error_string = callbacks->destroy_requester(requester, &rmw_free);
if (error_string) {
std::stringstream ss;
ss << "failed to destroy requester: " << error_string << ", at: " <<
__FILE__ << ":" << __LINE__ << '\n';
(std::cerr << ss.str()).flush();
}
}
if (client_info) {
rmw_free(client_info);
}
if (client) {
rmw_client_free(client);
}
return nullptr;
}
rmw_client_t *
rmw_client_allocate()
{
// Could be overridden with custom (maybe static) client struct allocator
return (rmw_client_t *)rmw_allocate(sizeof(rmw_client_t));
}
rmw_subscription_t *
rmw_subscription_allocate()
{
// Could be overridden with custom (maybe static) subscription struct allocator
return (rmw_subscription_t *)rmw_allocate(sizeof(rmw_subscription_t));
}
rmw_publisher_t *
rmw_publisher_allocate()
{
// Could be overridden with custom (maybe static) publisher struct allocator
return (rmw_publisher_t *)rmw_allocate(sizeof(rmw_publisher_t));
}
rmw_node_t *
rmw_node_allocate()
{
// Could be overridden with custom (maybe static) node struct allocator
return (rmw_node_t *)rmw_allocate(sizeof(rmw_node_t));
}
rmw_node_t *
rmw_create_node(const char * name, size_t domain_id)
{
if (!name) {
RMW_SET_ERROR_MSG("name is null");
return nullptr;
}
DDS::DomainParticipantFactory_var dp_factory = DDS::DomainParticipantFactory::get_instance();
if (!dp_factory) {
RMW_SET_ERROR_MSG("failed to get domain participant factory");
return nullptr;
}
DDS::DomainId_t domain = static_cast<DDS::DomainId_t>(domain_id);
DDS::DomainParticipant * participant = nullptr;
// Make sure that the OSPL_URI is set, otherwise node creation will fail.
char * ospl_uri = nullptr;
const char * ospl_uri_env = "OSPL_URI";
#ifndef _WIN32
ospl_uri = getenv(ospl_uri_env);
#else
size_t ospl_uri_size;
_dupenv_s(&ospl_uri, &ospl_uri_size, ospl_uri_env);
#endif
if (!ospl_uri) {
RMW_SET_ERROR_MSG("OSPL_URI not set");
return nullptr;
} else {
#ifdef _WIN32
free(ospl_uri);
#endif
}
// Ensure the ROS_DOMAIN_ID env variable is set, otherwise parsing of the config may fail.
// Also make sure the it is set to the domain_id passed in, otherwise it will fail.
// But first backup the current ROS_DOMAIN_ID.
char * ros_domain_id = nullptr;
const char * env_var = "ROS_DOMAIN_ID";
#ifndef _WIN32
ros_domain_id = getenv(env_var);
#else
size_t ros_domain_id_size;
_dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
#endif
// On Windows, setting the ROS_DOMAIN_ID does not fix the problem, so error early.
#ifdef _WIN32
if (!ros_domain_id) {
RMW_SET_ERROR_MSG("environment variable ROS_DOMAIN_ID is not set");
fprintf(stderr, "[rmw_opensplice_cpp]: error: %s\n", rmw_get_error_string_safe());
return nullptr;
}
#endif
// Set the ROS_DOMAIN_ID explicitly (if not Windows).
#ifndef _WIN32
auto domain_id_as_string = std::to_string(domain_id);
int ret = 0;
ret = setenv(env_var, domain_id_as_string.c_str(), 1);
if (0 != ret) {
RMW_SET_ERROR_MSG("failed to set the ROS_DOMAIN_ID");
return nullptr;
}
#endif
participant = dp_factory->create_participant(
domain, PARTICIPANT_QOS_DEFAULT, NULL, DDS::STATUS_MASK_NONE);
if (!participant) {
RMW_SET_ERROR_MSG("failed to create domain participant");
return NULL;
}
// Restore the ROS_DOMAIN_ID if necessary (and not Windows).
#ifndef _WIN32
if (ros_domain_id) {
ret = setenv(env_var, ros_domain_id, 1);
if (0 != ret) {
RMW_SET_ERROR_MSG("failed to reset the ROS_DOMAIN_ID");
return nullptr;
}
} else {
// Otherwise unset it again.
ret = unsetenv(env_var);
if (0 != ret) {
RMW_SET_ERROR_MSG("failed to unset the ROS_DOMAIN_ID");
return nullptr;
}
}
#endif
rmw_node_t * node = nullptr;
OpenSpliceStaticNodeInfo * node_info = nullptr;
CustomPublisherListener * publisher_listener = nullptr;
CustomSubscriberListener * subscriber_listener = nullptr;
void * buf = nullptr;
DDS::DataReader * data_reader = nullptr;
DDS::PublicationBuiltinTopicDataDataReader * builtin_publication_datareader = nullptr;
DDS::SubscriptionBuiltinTopicDataDataReader * builtin_subscription_datareader = nullptr;
DDS::Subscriber * builtin_subscriber = participant->get_builtin_subscriber();
if (!builtin_subscriber) {
RMW_SET_ERROR_MSG("builtin subscriber handle is null");
goto fail;
}
// setup publisher listener
data_reader = builtin_subscriber->lookup_datareader("DCPSPublication");
builtin_publication_datareader =
DDS::PublicationBuiltinTopicDataDataReader::_narrow(data_reader);
if (!builtin_publication_datareader) {
RMW_SET_ERROR_MSG("builtin publication datareader handle is null");
goto fail;
}
buf = rmw_allocate(sizeof(CustomPublisherListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(publisher_listener, buf, goto fail, CustomPublisherListener)
buf = nullptr;
builtin_publication_datareader->set_listener(publisher_listener, DDS::DATA_AVAILABLE_STATUS);
data_reader = builtin_subscriber->lookup_datareader("DCPSSubscription");
builtin_subscription_datareader =
DDS::SubscriptionBuiltinTopicDataDataReader::_narrow(data_reader);
if (!builtin_subscription_datareader) {
RMW_SET_ERROR_MSG("builtin subscription datareader handle is null");
goto fail;
}
// setup subscriber listener
buf = rmw_allocate(sizeof(CustomSubscriberListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(subscriber_listener, buf, goto fail, CustomSubscriberListener)
buf = nullptr;
builtin_subscription_datareader->set_listener(subscriber_listener, DDS::DATA_AVAILABLE_STATUS);
node = rmw_node_allocate();
if (!node) {
RMW_SET_ERROR_MSG("failed to allocate rmw_node_t");
goto fail;
}
node->name = reinterpret_cast<const char *>(rmw_allocate(sizeof(char) * strlen(name) + 1));
if (!node->name) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
memcpy(const_cast<char *>(node->name), name, strlen(name) + 1);
buf = rmw_allocate(sizeof(OpenSpliceStaticNodeInfo));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(node_info, buf, goto fail, OpenSpliceStaticNodeInfo)
buf = nullptr;
node_info->participant = participant;
node_info->publisher_listener = publisher_listener;
node_info->subscriber_listener = subscriber_listener;
node->implementation_identifier = opensplice_cpp_identifier;
node->data = node_info;
return node;
fail:
if (participant) {
if (dp_factory->delete_participant(participant) != DDS::RETCODE_OK) {
std::stringstream ss;
ss << "leaking domain participant while handling failure at: " <<
__FILE__ << ":" << __LINE__ << '\n';
(std::cerr << ss.str()).flush();
}
}
if (publisher_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
publisher_listener->~CustomPublisherListener(), CustomPublisherListener)
rmw_free(publisher_listener);
}
if (subscriber_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener)
rmw_free(subscriber_listener);
}
if (node_info) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->~OpenSpliceStaticNodeInfo(), OpenSpliceStaticNodeInfo)
rmw_free(node_info);
}
if (buf) {
rmw_free(buf);
}
if (node) {
if (node->name) {
rmw_free(const_cast<char *>(node->name));
}
rmw_node_free(node);
}
return nullptr;
}
rmw_wait_set_t *
create_wait_set(
const char * implementation_identifier,
rmw_context_t * context,
size_t max_conditions)
{
RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL);
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
init context,
context->implementation_identifier,
implementation_identifier,
// TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored
return nullptr);
rmw_wait_set_t * wait_set = rmw_wait_set_allocate();
ConnextWaitSetInfo * wait_set_info = nullptr;
// From here onward, error results in unrolling in the goto fail block.
if (!wait_set) {
RMW_SET_ERROR_MSG("failed to allocate wait set");
goto fail;
}
wait_set->implementation_identifier = implementation_identifier;
wait_set->data = rmw_allocate(sizeof(ConnextWaitSetInfo));
wait_set_info = static_cast<ConnextWaitSetInfo *>(wait_set->data);
if (!wait_set_info) {
RMW_SET_ERROR_MSG("failed to allocate wait set");
goto fail;
}
wait_set_info->wait_set = static_cast<DDS::WaitSet *>(rmw_allocate(sizeof(DDS::WaitSet)));
if (!wait_set_info->wait_set) {
RMW_SET_ERROR_MSG("failed to allocate wait set");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
wait_set_info->wait_set, wait_set_info->wait_set, goto fail, DDS::WaitSet, )
// Now allocate storage for the ConditionSeq objects
wait_set_info->active_conditions =
static_cast<DDS::ConditionSeq *>(rmw_allocate(sizeof(DDS::ConditionSeq)));
if (!wait_set_info->active_conditions) {
RMW_SET_ERROR_MSG("failed to allocate active_conditions sequence");
goto fail;
}
wait_set_info->attached_conditions =
static_cast<DDS::ConditionSeq *>(rmw_allocate(sizeof(DDS::ConditionSeq)));
if (!wait_set_info->attached_conditions) {
RMW_SET_ERROR_MSG("failed to allocate attached_conditions sequence");
goto fail;
}
// If max_conditions is greater than zero, re-allocate both ConditionSeqs to max_conditions
if (max_conditions > 0) {
RMW_TRY_PLACEMENT_NEW(
wait_set_info->active_conditions, wait_set_info->active_conditions, goto fail,
DDS::ConditionSeq, static_cast<DDS::Long>(max_conditions))
RMW_TRY_PLACEMENT_NEW(
wait_set_info->attached_conditions, wait_set_info->attached_conditions, goto fail,
DDS::ConditionSeq,
static_cast<DDS::Long>(max_conditions))
} else {
