diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index d08b4ec7..1a6fc9aa 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -20,6 +20,7 @@ #include "rmw/rmw.h" #include "rmw/types.h" +#include "rmw_connext_shared_cpp/create_topic.hpp" #include "rmw_connext_shared_cpp/qos.hpp" #include "rmw_connext_shared_cpp/types.hpp" @@ -122,7 +123,6 @@ rmw_create_publisher( DDS::Publisher * dds_publisher = nullptr; DDS::DataWriter * topic_writer = nullptr; DDS::Topic * topic = nullptr; - DDS::TopicDescription * topic_description = nullptr; void * info_buf = nullptr; void * listener_buf = nullptr; ConnextPublisherListener * publisher_listener = nullptr; @@ -192,33 +192,10 @@ rmw_create_publisher( goto fail; } - topic_description = participant->lookup_topicdescription(topic_str); - if (!topic_description) { - DDS::TopicQos default_topic_qos; - status = participant->get_default_topic_qos(default_topic_qos); - if (status != DDS::RETCODE_OK) { - RMW_SET_ERROR_MSG("failed to get default topic qos"); - goto fail; - } - - topic = participant->create_topic( - topic_str, type_name.c_str(), - default_topic_qos, NULL, DDS::STATUS_MASK_NONE); - if (!topic) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to create topic '%s' for node namespace='%s' name='%s'", - topic_name, node->namespace_, node->name); - goto fail; - } - } else { - DDS::Duration_t timeout = DDS::Duration_t::from_seconds(0); - topic = participant->find_topic(topic_str, timeout); - if (!topic) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to find topic '%s' for node namespace='%s' name='%s'", - topic_name, node->namespace_, node->name); - goto fail; - } + topic = rmw_connext_shared_cpp::create_topic(node, topic_name, topic_str, type_name.c_str()); + if (!topic) { + // error already set + goto fail; } DDS::String_free(topic_str); topic_str = nullptr; diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index c22a13bc..a90f1b01 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -19,6 +19,7 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_connext_shared_cpp/create_topic.hpp" #include "rmw_connext_shared_cpp/qos.hpp" #include "rmw_connext_shared_cpp/types.hpp" @@ -115,7 +116,6 @@ rmw_create_subscription( DDS::ReturnCode_t status; DDS::Subscriber * dds_subscriber = nullptr; DDS::Topic * topic = nullptr; - DDS::TopicDescription * topic_description = nullptr; DDS::DataReader * topic_reader = nullptr; DDS::ReadCondition * read_condition = nullptr; void * info_buf = nullptr; @@ -186,33 +186,10 @@ rmw_create_subscription( goto fail; } - topic_description = participant->lookup_topicdescription(topic_str); - if (!topic_description) { - DDS::TopicQos default_topic_qos; - status = participant->get_default_topic_qos(default_topic_qos); - if (status != DDS::RETCODE_OK) { - RMW_SET_ERROR_MSG("failed to get default topic qos"); - goto fail; - } - - topic = participant->create_topic( - topic_str, type_name.c_str(), - default_topic_qos, NULL, DDS::STATUS_MASK_NONE); - if (!topic) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to create topic '%s' for node namespace='%s' name='%s'", - topic_name, node->namespace_, node->name); - goto fail; - } - } else { - DDS::Duration_t timeout = DDS::Duration_t::from_seconds(0); - topic = participant->find_topic(topic_str, timeout); - if (!topic) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to find topic '%s' for node namespace='%s' name='%s'", - topic_name, node->namespace_, node->name); - goto fail; - } + topic = rmw_connext_shared_cpp::create_topic(node, topic_name, topic_str, type_name.c_str()); + if (!topic) { + // error already set + goto fail; } DDS::String_free(topic_str); topic_str = nullptr; diff --git a/rmw_connext_dynamic_cpp/src/functions.cpp b/rmw_connext_dynamic_cpp/src/functions.cpp index bcc10976..2ff75b7d 100644 --- a/rmw_connext_dynamic_cpp/src/functions.cpp +++ b/rmw_connext_dynamic_cpp/src/functions.cpp @@ -76,6 +76,7 @@ #include "rosidl_typesupport_introspection_c/service_introspection.h" #include "rosidl_typesupport_introspection_c/visibility_control.h" +#include "rmw_connext_shared_cpp/create_topic.hpp" #include "rmw_connext_shared_cpp/shared_functions.hpp" #include "rmw_connext_shared_cpp/topic_endpoint_info.hpp" #include "rmw_connext_shared_cpp/types.hpp" @@ -401,7 +402,6 @@ rmw_create_publisher( DDS_PublisherQos publisher_qos; DDSPublisher * dds_publisher = nullptr; DDSTopic * topic = nullptr; - DDSTopicDescription * topic_description = nullptr; DDS_DataWriterQos datawriter_qos; DDSDataWriter * topic_writer = nullptr; DDSDynamicDataWriter * dynamic_writer = nullptr; @@ -494,28 +494,10 @@ rmw_create_publisher( goto fail; } - topic_description = participant->lookup_topicdescription(topic_str); - if (!topic_description) { - DDS_TopicQos default_topic_qos; - status = participant->get_default_topic_qos(default_topic_qos); - if (status != DDS_RETCODE_OK) { - RMW_SET_ERROR_MSG("failed to get default topic qos"); - goto fail; - } - - topic = participant->create_topic( - topic_str, type_name.c_str(), default_topic_qos, NULL, DDS_STATUS_MASK_NONE); - if (!topic) { - RMW_SET_ERROR_MSG("failed to create topic"); - goto fail; - } - } else { - DDS_Duration_t timeout = DDS_Duration_t::from_seconds(0); - topic = participant->find_topic(topic_str, timeout); - if (!topic) { - RMW_SET_ERROR_MSG("failed to find topic"); - goto fail; - } + topic = rmw_connext_shared_cpp::create_topic(node, topic_name, topic_str, type_name.c_str()); + if (!topic) { + // error already set + goto fail; } if (!get_datawriter_qos(participant, *qos_profile, datawriter_qos)) { @@ -1068,7 +1050,6 @@ rmw_create_subscription( DDS_SubscriberQos subscriber_qos; DDSSubscriber * dds_subscriber = nullptr; DDSTopic * topic; - DDSTopicDescription * topic_description = nullptr; DDSDataReader * topic_reader = nullptr; DDSReadCondition * read_condition = nullptr; DDSDynamicDataReader * dynamic_reader = nullptr; @@ -1125,28 +1106,10 @@ rmw_create_subscription( goto fail; } - topic_description = participant->lookup_topicdescription(topic_name); - if (!topic_description) { - DDS_TopicQos default_topic_qos; - status = participant->get_default_topic_qos(default_topic_qos); - if (status != DDS_RETCODE_OK) { - RMW_SET_ERROR_MSG("failed to get default topic qos"); - goto fail; - } - - topic = participant->create_topic( - topic_name, type_name.c_str(), default_topic_qos, NULL, DDS_STATUS_MASK_NONE); - if (!topic) { - RMW_SET_ERROR_MSG("failed to create topic"); - goto fail; - } - } else { - DDS_Duration_t timeout = DDS_Duration_t::from_seconds(0); - topic = participant->find_topic(topic_name, timeout); - if (!topic) { - RMW_SET_ERROR_MSG("failed to find topic"); - goto fail; - } + topic = rmw_connext_shared_cpp::create_topic(node, topic_name, topic_str, type_name.c_str()); + if (!topic) { + // error already set + goto fail; } if (!get_datareader_qos(participant, *qos_profile, datareader_qos)) { diff --git a/rmw_connext_shared_cpp/CMakeLists.txt b/rmw_connext_shared_cpp/CMakeLists.txt index 82aab51e..226087c5 100644 --- a/rmw_connext_shared_cpp/CMakeLists.txt +++ b/rmw_connext_shared_cpp/CMakeLists.txt @@ -38,6 +38,7 @@ add_library( SHARED src/condition_error.cpp src/count.cpp + src/create_topic.cpp src/demangle.cpp src/event.cpp src/event_converter.cpp diff --git a/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/create_topic.hpp b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/create_topic.hpp new file mode 100644 index 00000000..f693f205 --- /dev/null +++ b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/create_topic.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_CONNEXT_SHARED_CPP__CREATE_TOPIC_HPP_ +#define RMW_CONNEXT_SHARED_CPP__CREATE_TOPIC_HPP_ + +#include "rmw/types.h" +#include "rmw_connext_shared_cpp/ndds_include.hpp" +#include "rmw_connext_shared_cpp/visibility_control.h" + +namespace rmw_connext_shared_cpp +{ + +/// Create a DDS::Topic from a node +/** + * \pre node must not be null. + * \pre node must be a valid node, i.e. node->data is not nullptr. + * + * \param[in] node rmw node structure. + * \param[in] topic_name ros topic name. + * \param[in] dds_topic_name demangled topic name. + * \param[in] dds_topic_type demangled topic type name. + * \return created DDS::Topic, nullptr on failure. + */ +RMW_CONNEXT_SHARED_CPP_PUBLIC +DDS::Topic * +create_topic( + const rmw_node_t * node, + const char * topic_name, + const char * dds_topic_name, + const char * dds_topic_type); + +} // namespace rmw_connext_shared_cpp + +#endif // RMW_CONNEXT_SHARED_CPP__CREATE_TOPIC_HPP_ diff --git a/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/types.hpp b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/types.hpp index 68a2853d..39ecc92b 100644 --- a/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/types.hpp +++ b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/types.hpp @@ -149,6 +149,7 @@ struct ConnextNodeInfo CustomPublisherListener * publisher_listener; CustomSubscriberListener * subscriber_listener; rmw_guard_condition_t * graph_guard_condition; + std::mutex topic_creation_mutex; }; struct ConnextPublisherGID diff --git a/rmw_connext_shared_cpp/src/create_topic.cpp b/rmw_connext_shared_cpp/src/create_topic.cpp new file mode 100644 index 00000000..771ef85f --- /dev/null +++ b/rmw_connext_shared_cpp/src/create_topic.cpp @@ -0,0 +1,76 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_connext_shared_cpp/create_topic.hpp" + +#include +#include + +#include "rmw/error_handling.h" +#include "rmw/types.h" + +#include "rmw_connext_shared_cpp/types.hpp" +#include "rmw_connext_shared_cpp/ndds_include.hpp" + +DDS::Topic * +rmw_connext_shared_cpp::create_topic( + const rmw_node_t * node, + const char * topic_name, + const char * dds_topic_name, + const char * dds_topic_type) +{ + // This function is internal, and should be called from functions that already verified + // the node validity. + assert(node); + assert(node->data); + + auto node_info = static_cast(node->data); + auto participant = static_cast(node_info->participant); + + std::lock_guard guard(node_info->topic_creation_mutex); + + DDS::TopicDescription * topic_description = + participant->lookup_topicdescription(dds_topic_name); + + DDS::Topic * topic = nullptr; + DDS::ReturnCode_t status = DDS::RETCODE_ERROR; + if (!topic_description) { + DDS::TopicQos default_topic_qos; + status = participant->get_default_topic_qos(default_topic_qos); + if (status != DDS::RETCODE_OK) { + RMW_SET_ERROR_MSG("failed to get default topic qos"); + return nullptr; + } + + topic = participant->create_topic( + dds_topic_name, dds_topic_type, + default_topic_qos, nullptr, DDS::STATUS_MASK_NONE); + if (!topic) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to create topic '%s' for node namespace='%s' name='%s'", + topic_name, node->namespace_, node->name); + return nullptr; + } + } else { + DDS::Duration_t timeout = DDS::Duration_t::from_seconds(0); + topic = participant->find_topic(dds_topic_name, timeout); + if (!topic) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to find topic '%s' for node namespace='%s' name='%s'", + topic_name, node->namespace_, node->name); + return nullptr; + } + } + return topic; +}