From 36bfaa48ec9f23ccd6f80536b6d91ef612d847e3 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 15 Dec 2023 17:49:51 +0000 Subject: [PATCH 1/3] Started support for humble --- rmw_opendds_cpp/CMakeLists.txt | 2 + .../include/rmw_opendds_cpp/init.hpp | 17 +- rmw_opendds_cpp/package.xml | 4 +- rmw_opendds_cpp/src/OpenDDSNode.cpp | 337 ++++++++++-------- rmw_opendds_cpp/src/init.cpp | 47 +-- rmw_opendds_cpp/src/qos.cpp | 19 + rmw_opendds_cpp/src/rmw_client.cpp | 64 ++++ rmw_opendds_cpp/src/rmw_event.cpp | 17 + rmw_opendds_cpp/src/rmw_features.cpp | 7 + .../src/rmw_get_network_flow_endpoints.cpp | 35 ++ rmw_opendds_cpp/src/rmw_node.cpp | 6 +- rmw_opendds_cpp/src/rmw_publisher.cpp | 13 + rmw_opendds_cpp/src/rmw_server.cpp | 40 +++ rmw_opendds_cpp/src/rmw_subscription.cpp | 44 +++ 14 files changed, 466 insertions(+), 186 deletions(-) create mode 100644 rmw_opendds_cpp/src/rmw_features.cpp create mode 100644 rmw_opendds_cpp/src/rmw_get_network_flow_endpoints.cpp diff --git a/rmw_opendds_cpp/CMakeLists.txt b/rmw_opendds_cpp/CMakeLists.txt index 9c94993..51cda04 100644 --- a/rmw_opendds_cpp/CMakeLists.txt +++ b/rmw_opendds_cpp/CMakeLists.txt @@ -96,7 +96,9 @@ add_library( src/rmw_client.cpp src/rmw_compare_gid_equals.cpp src/rmw_event.cpp + src/rmw_features.cpp src/rmw_get_implementation_identifier.cpp + src/rmw_get_network_flow_endpoints.cpp src/rmw_get_serialization_format.cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp diff --git a/rmw_opendds_cpp/include/rmw_opendds_cpp/init.hpp b/rmw_opendds_cpp/include/rmw_opendds_cpp/init.hpp index 8d92572..f1d3a2a 100644 --- a/rmw_opendds_cpp/include/rmw_opendds_cpp/init.hpp +++ b/rmw_opendds_cpp/include/rmw_opendds_cpp/init.hpp @@ -21,21 +21,20 @@ #include -struct rmw_context_impl_t -{ - rmw_context_impl_t(); - ~rmw_context_impl_t(); +struct rmw_context_impl_s { + rmw_context_impl_s(); + ~rmw_context_impl_s(); DDS::DomainParticipantFactory_var dpf_; }; RMW_OPENDDS_CPP_PUBLIC -rmw_ret_t init(rmw_context_t& context); +rmw_ret_t init(rmw_context_t &context); RMW_OPENDDS_CPP_PUBLIC -rmw_ret_t shutdown(rmw_context_t& context); +rmw_ret_t shutdown(rmw_context_t &context); -bool is_zero_initialized(const rmw_init_options_t * o); +bool is_zero_initialized(const rmw_init_options_t *o); -bool is_zero_initialized(const rmw_context_t * c); +bool is_zero_initialized(const rmw_context_t *c); -#endif // RMW_OPENDDS_CPP__INIT_HPP_ +#endif // RMW_OPENDDS_CPP__INIT_HPP_ diff --git a/rmw_opendds_cpp/package.xml b/rmw_opendds_cpp/package.xml index 4789d02..fcedc57 100644 --- a/rmw_opendds_cpp/package.xml +++ b/rmw_opendds_cpp/package.xml @@ -13,10 +13,10 @@ ament_cmake rosidl_cmake - opendds_cmake_module + rcutils rmw - opendds + rosidl_generator_c rosidl_generator_cpp rosidl_generator_dds_idl diff --git a/rmw_opendds_cpp/src/OpenDDSNode.cpp b/rmw_opendds_cpp/src/OpenDDSNode.cpp index bf85510..047ff8a 100644 --- a/rmw_opendds_cpp/src/OpenDDSNode.cpp +++ b/rmw_opendds_cpp/src/OpenDDSNode.cpp @@ -12,21 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include +#include #include #include -#include -#include #include #include +#include #include -#include #include -#include #include -#include +#include +#include +#include #ifdef OPENDDS_SECURITY #include #endif @@ -37,12 +38,11 @@ #include #include #include -#include -#include #include +#include +#include -OpenDDSNode* OpenDDSNode::from(const rmw_node_t * node) -{ +OpenDDSNode *OpenDDSNode::from(const rmw_node_t *node) { RMW_CHECK_FOR_NULL_WITH_MSG(node, "node is null", return nullptr); if (!check_impl_id(node->implementation_identifier)) { return nullptr; @@ -52,8 +52,8 @@ OpenDDSNode* OpenDDSNode::from(const rmw_node_t * node) return dds_node; } -bool OpenDDSNode::validate_name_namespace(const char * node_name, const char * node_namespace) -{ +bool OpenDDSNode::validate_name_namespace(const char *node_name, + const char *node_namespace) { if (!node_name || strlen(node_name) == 0) { RMW_SET_ERROR_MSG("node_name is null or empty"); return false; @@ -65,24 +65,27 @@ bool OpenDDSNode::validate_name_namespace(const char * node_name, const char * n return true; } -void OpenDDSNode::add_pub(const DDS::InstanceHandle_t& pub, const std::string& topic_name, const std::string& type_name) -{ +void OpenDDSNode::add_pub(const DDS::InstanceHandle_t &pub, + const std::string &topic_name, + const std::string &type_name) { DDS::GUID_t part_guid = dpi_->get_repoid(dp_->get_instance_handle()); DDS::GUID_t guid = dpi_->get_repoid(pub); - pub_listener_->add_information(part_guid, guid, topic_name, type_name, EntityType::Publisher); + pub_listener_->add_information(part_guid, guid, topic_name, type_name, + EntityType::Publisher); pub_listener_->trigger_graph_guard_condition(); } -void OpenDDSNode::add_sub(const DDS::InstanceHandle_t& sub, const std::string& topic_name, const std::string& type_name) -{ +void OpenDDSNode::add_sub(const DDS::InstanceHandle_t &sub, + const std::string &topic_name, + const std::string &type_name) { DDS::GUID_t part_guid = dpi_->get_repoid(dp_->get_instance_handle()); DDS::GUID_t guid = dpi_->get_repoid(sub); - sub_listener_->add_information(part_guid, guid, topic_name, type_name, EntityType::Subscriber); + sub_listener_->add_information(part_guid, guid, topic_name, type_name, + EntityType::Subscriber); sub_listener_->trigger_graph_guard_condition(); } -bool OpenDDSNode::remove_pub(const DDS::InstanceHandle_t& pub) -{ +bool OpenDDSNode::remove_pub(const DDS::InstanceHandle_t &pub) { DDS::GUID_t guid = dpi_->get_repoid(pub); if (pub_listener_->remove_information(guid, EntityType::Publisher)) { pub_listener_->trigger_graph_guard_condition(); @@ -90,8 +93,7 @@ bool OpenDDSNode::remove_pub(const DDS::InstanceHandle_t& pub) return true; } -bool OpenDDSNode::remove_sub(const DDS::InstanceHandle_t& sub) -{ +bool OpenDDSNode::remove_sub(const DDS::InstanceHandle_t &sub) { DDS::GUID_t guid = dpi_->get_repoid(sub); if (sub_listener_->remove_information(guid, EntityType::Subscriber)) { sub_listener_->trigger_graph_guard_condition(); @@ -99,8 +101,7 @@ bool OpenDDSNode::remove_sub(const DDS::InstanceHandle_t& sub) return true; } -rmw_ret_t OpenDDSNode::count_publishers(const char * topic_name, size_t * count) -{ +rmw_ret_t OpenDDSNode::count_publishers(const char *topic_name, size_t *count) { if (!topic_name) { RMW_SET_ERROR_MSG("topic name is null"); return RMW_RET_ERROR; @@ -113,8 +114,8 @@ rmw_ret_t OpenDDSNode::count_publishers(const char * topic_name, size_t * count) return RMW_RET_OK; } -rmw_ret_t OpenDDSNode::count_subscribers(const char * topic_name, size_t * count) -{ +rmw_ret_t OpenDDSNode::count_subscribers(const char *topic_name, + size_t *count) { if (!topic_name) { RMW_SET_ERROR_MSG("topic name is null"); return RMW_RET_ERROR; @@ -127,8 +128,9 @@ rmw_ret_t OpenDDSNode::count_subscribers(const char * topic_name, size_t * count return RMW_RET_OK; } -rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t * names, rcutils_string_array_t * namespaces, rcutils_string_array_t * enclaves) const -{ +rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t *names, + rcutils_string_array_t *namespaces, + rcutils_string_array_t *enclaves) const { if (rmw_check_zero_rmw_string_array(names) != RMW_RET_OK) { RMW_SET_ERROR_MSG("rmw_check_zero_rmw_string_array(names) failed."); return RMW_RET_INVALID_ARGUMENT; @@ -142,17 +144,20 @@ rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t * names, rcutils_string_ RMW_SET_ERROR_MSG("unable to fetch discovered participants."); return RMW_RET_ERROR; } - const auto length = handles.length() + 1; // add yourself + const auto length = handles.length() + 1; // add yourself rcutils_allocator_t allocator = rcutils_get_default_allocator(); try { - if (rcutils_string_array_init(names, length, &allocator) != RCUTILS_RET_OK) { + if (rcutils_string_array_init(names, length, &allocator) != + RCUTILS_RET_OK) { throw std::runtime_error(rcutils_get_error_string().str); } - if (rcutils_string_array_init(namespaces, length, &allocator) != RCUTILS_RET_OK) { + if (rcutils_string_array_init(namespaces, length, &allocator) != + RCUTILS_RET_OK) { throw std::runtime_error(rcutils_get_error_string().str); } if (enclaves) { - if (rcutils_string_array_init(enclaves, length, &allocator) != RCUTILS_RET_OK) { + if (rcutils_string_array_init(enclaves, length, &allocator) != + RCUTILS_RET_OK) { throw std::runtime_error(rcutils_get_error_string().str); } } @@ -167,7 +172,8 @@ rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t * names, rcutils_string_ if (enclaves) { enclaves->data[0] = rcutils_strdup(context_.options.enclave, allocator); if (!enclaves->data[0]) { - throw std::runtime_error("could not allocate memory for a enclave name"); + throw std::runtime_error( + "could not allocate memory for a enclave name"); } } for (CORBA::ULong i = 1; i < length; ++i) { @@ -177,20 +183,23 @@ rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t * names, rcutils_string_ std::string ns; std::string enclave; if (DDS::RETCODE_OK == dds_ret) { - auto data = static_cast(pbtd.user_data.value.get_buffer()); + auto data = + static_cast(pbtd.user_data.value.get_buffer()); std::vector kv(data, data + pbtd.user_data.value.length()); auto map = rmw::impl::cpp::parse_key_value(kv); auto name_found = map.find("name"); auto ns_found = map.find("namespace"); auto enclave_found = map.find("enclave"); if (name_found != map.end()) { - name = std::string(name_found->second.begin(), name_found->second.end()); + name = + std::string(name_found->second.begin(), name_found->second.end()); } if (name_found != map.end()) { ns = std::string(ns_found->second.begin(), ns_found->second.end()); } if (enclave_found != map.end()) { - enclave = std::string(enclave_found->second.begin(), enclave_found->second.end()); + enclave = std::string(enclave_found->second.begin(), + enclave_found->second.end()); } } if (name.empty()) { @@ -205,17 +214,19 @@ rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t * names, rcutils_string_ } namespaces->data[i] = rcutils_strdup(ns.c_str(), allocator); if (!namespaces->data[i]) { - throw std::runtime_error("could not allocate memory for node namespace"); + throw std::runtime_error( + "could not allocate memory for node namespace"); } if (enclaves) { enclaves->data[i] = rcutils_strdup(enclave.c_str(), allocator); if (!enclaves->data[i]) { - throw std::runtime_error("could not allocate memory for enclave namespace"); + throw std::runtime_error( + "could not allocate memory for enclave namespace"); } } } return RMW_RET_OK; - } catch (const std::exception& e) { + } catch (const std::exception &e) { RMW_SET_ERROR_MSG(e.what()); } catch (...) { RMW_SET_ERROR_MSG("OpenDDSNode::get_names failed."); @@ -232,9 +243,11 @@ rmw_ret_t OpenDDSNode::get_names(rcutils_string_array_t * names, rcutils_string_ return RMW_RET_ERROR; } -rmw_ret_t OpenDDSNode::get_pub_names_types(rmw_names_and_types_t * nt, - const char * node_name, const char * node_namespace, bool no_demangle, rcutils_allocator_t * allocator) const -{ +rmw_ret_t +OpenDDSNode::get_pub_names_types(rmw_names_and_types_t *nt, + const char *node_name, + const char *node_namespace, bool no_demangle, + rcutils_allocator_t *allocator) const { rmw_ret_t ret = rmw_names_and_types_check_zero(nt); if (ret != RMW_RET_OK) { return ret; @@ -252,9 +265,11 @@ rmw_ret_t OpenDDSNode::get_pub_names_types(rmw_names_and_types_t * nt, return copy_topic_names_types(nt, ntm, no_demangle, allocator); } -rmw_ret_t OpenDDSNode::get_sub_names_types(rmw_names_and_types_t * nt, - const char * node_name, const char * node_namespace, bool no_demangle, rcutils_allocator_t * allocator) const -{ +rmw_ret_t +OpenDDSNode::get_sub_names_types(rmw_names_and_types_t *nt, + const char *node_name, + const char *node_namespace, bool no_demangle, + rcutils_allocator_t *allocator) const { rmw_ret_t ret = rmw_names_and_types_check_zero(nt); if (ret != RMW_RET_OK) { return ret; @@ -272,8 +287,9 @@ rmw_ret_t OpenDDSNode::get_sub_names_types(rmw_names_and_types_t * nt, return copy_topic_names_types(nt, ntm, no_demangle, allocator); } -rmw_ret_t OpenDDSNode::get_topic_names_types(rmw_names_and_types_t * nt, bool no_demangle, rcutils_allocator_t * allocator) const -{ +rmw_ret_t +OpenDDSNode::get_topic_names_types(rmw_names_and_types_t *nt, bool no_demangle, + rcutils_allocator_t *allocator) const { rmw_ret_t ret = rmw_names_and_types_check_zero(nt); if (ret != RMW_RET_OK) { return ret; @@ -285,8 +301,9 @@ rmw_ret_t OpenDDSNode::get_topic_names_types(rmw_names_and_types_t * nt, bool no return copy_topic_names_types(nt, ntm, no_demangle, allocator); } -rmw_ret_t OpenDDSNode::get_service_names_types(rmw_names_and_types_t * nt, rcutils_allocator_t * allocator) const -{ +rmw_ret_t +OpenDDSNode::get_service_names_types(rmw_names_and_types_t *nt, + rcutils_allocator_t *allocator) const { rmw_ret_t ret = rmw_names_and_types_check_zero(nt); if (ret != RMW_RET_OK) { return ret; @@ -302,9 +319,10 @@ rmw_ret_t OpenDDSNode::get_service_names_types(rmw_names_and_types_t * nt, rcuti return copy_service_names_types(nt, ntm, allocator); } -rmw_ret_t OpenDDSNode::get_service_names_types(rmw_names_and_types_t * nt, - const char * node_name, const char * node_namespace, const char* suffix, rcutils_allocator_t* allocator) const -{ +rmw_ret_t OpenDDSNode::get_service_names_types( + rmw_names_and_types_t *nt, const char *node_name, + const char *node_namespace, const char *suffix, + rcutils_allocator_t *allocator) const { rmw_ret_t ret = rmw_names_and_types_check_zero(nt); if (ret != RMW_RET_OK) { return ret; @@ -326,16 +344,11 @@ rmw_ret_t OpenDDSNode::get_service_names_types(rmw_names_and_types_t * nt, return copy_service_names_types(nt, ntm, allocator); } -OpenDDSNode::OpenDDSNode(rmw_context_t & context, const char * name, const char * name_space) - : context_(context) - , name_(name ? name : "") - , namespace_(name_space ? name_space : "") - , gc_(nullptr) - , pub_listener_(nullptr) - , sub_listener_(nullptr) - , dp_() - , dpi_(nullptr) -{ +OpenDDSNode::OpenDDSNode(rmw_context_t &context, const char *name, + const char *name_space) + : context_(context), name_(name ? name : ""), + namespace_(name_space ? name_space : ""), gc_(nullptr), + pub_listener_(nullptr), sub_listener_(nullptr), dp_(), dpi_(nullptr) { try { if (name_.empty()) { throw std::runtime_error("node name_ is null"); @@ -356,15 +369,19 @@ OpenDDSNode::OpenDDSNode(rmw_context_t & context, const char * name, const char if (!sub_listener_) { throw std::runtime_error("CustomSubscriberListener failed"); } - OpenDDS::DCPS::Discovery_rch disco = TheServiceParticipant->get_discovery(context.options.domain_id); - OpenDDS::RTPS::RtpsDiscovery_rch rtps_disco = OpenDDS::DCPS::dynamic_rchandle_cast(disco); - rtps_disco->use_xtypes(false); + OpenDDS::DCPS::Discovery_rch disco = + TheServiceParticipant->get_discovery(context.options.domain_id); + OpenDDS::RTPS::RtpsDiscovery_rch rtps_disco = + OpenDDS::DCPS::dynamic_rchandle_cast( + disco); + rtps_disco->use_xtypes(OpenDDS::RTPS::RtpsDiscoveryConfig::XTYPES_MINIMAL); dp_ = context.impl->dpf_->create_participant( - static_cast(context.options.domain_id), PARTICIPANT_QOS_DEFAULT, 0, 0); + static_cast(context.options.domain_id), + PARTICIPANT_QOS_DEFAULT, 0, 0); if (!dp_) { throw std::runtime_error("create_participant failed"); } - dpi_ = dynamic_cast(dp_.in()); + dpi_ = dynamic_cast(dp_.in()); if (!dpi_) { throw std::runtime_error("casting to DomainParticipantImpl failed"); } @@ -378,8 +395,10 @@ OpenDDSNode::OpenDDSNode(rmw_context_t & context, const char * name, const char throw std::runtime_error("get_builtin_subscriber failed"); } // setup publisher listener - DDS::DataReader_ptr dr = sub->lookup_datareader(OpenDDS::DCPS::BUILT_IN_PUBLICATION_TOPIC); - auto pub_dr = dynamic_cast(dr); + DDS::DataReader_ptr dr = + sub->lookup_datareader(OpenDDS::DCPS::BUILT_IN_PUBLICATION_TOPIC); + auto pub_dr = + dynamic_cast(dr); if (!pub_dr) { throw std::runtime_error("builtin publication datareader is null"); } @@ -387,13 +406,14 @@ OpenDDSNode::OpenDDSNode(rmw_context_t & context, const char * name, const char // setup subscriber listener dr = sub->lookup_datareader(OpenDDS::DCPS::BUILT_IN_SUBSCRIPTION_TOPIC); - auto sub_dr = dynamic_cast(dr); + auto sub_dr = + dynamic_cast(dr); if (!sub_dr) { throw std::runtime_error("builtin subscription datareader is null"); } sub_dr->set_listener(sub_listener_, DDS::DATA_AVAILABLE_STATUS); - } catch (const std::exception& e) { + } catch (const std::exception &e) { RMW_SET_ERROR_MSG(e.what()); cleanup(); throw; @@ -404,8 +424,7 @@ OpenDDSNode::OpenDDSNode(rmw_context_t & context, const char * name, const char } } -void OpenDDSNode::cleanup() -{ +void OpenDDSNode::cleanup() { if (dp_) { if (dp_->delete_contained_entities() != DDS::RETCODE_OK) { RMW_SET_ERROR_MSG("dp_->delete_contained_entities failed"); @@ -432,118 +451,141 @@ void OpenDDSNode::cleanup() } #ifdef OPENDDS_SECURITY -void append(DDS::PropertySeq& props, const char* name, const char* value, bool propagate = false) -{ +void append(DDS::PropertySeq &props, const char *name, const char *value, + bool propagate = false) { const DDS::Property_t prop = {name, value, propagate}; const unsigned int len = props.length(); props.length(len + 1); props[len] = prop; } -bool enable_security(const char * root, DDS::PropertySeq& props) -{ +bool enable_security(const char *root, DDS::PropertySeq &props) { try { rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * buf = rcutils_join_path(root, "identity_ca.cert.pem", allocator); + char *buf = rcutils_join_path(root, "identity_ca.cert.pem", allocator); if (buf) { append(props, DDS::Security::Properties::AuthIdentityCA, buf); allocator.deallocate(buf, allocator.state); buf = nullptr; - } else { throw std::string("failed to allocate memory for identity_ca_cert_fn"); } + } else { + throw std::string("failed to allocate memory for identity_ca_cert_fn"); + } buf = rcutils_join_path(root, "permissions_ca.cert.pem", allocator); if (buf) { append(props, DDS::Security::Properties::AccessPermissionsCA, buf); allocator.deallocate(buf, allocator.state); buf = nullptr; - } else { throw std::string("failed to allocate memory for permissions_ca_cert_fn"); } + } else { + throw std::string("failed to allocate memory for permissions_ca_cert_fn"); + } buf = rcutils_join_path(root, "cert.pem", allocator); if (buf) { append(props, DDS::Security::Properties::AuthIdentityCertificate, buf); allocator.deallocate(buf, allocator.state); buf = nullptr; - } else { throw std::string("failed to allocate memory for cert_fn"); } + } else { + throw std::string("failed to allocate memory for cert_fn"); + } buf = rcutils_join_path(root, "key.pem", allocator); if (buf) { append(props, DDS::Security::Properties::AuthPrivateKey, buf); allocator.deallocate(buf, allocator.state); buf = nullptr; - } else { throw std::string("failed to allocate memory for key_fn"); } + } else { + throw std::string("failed to allocate memory for key_fn"); + } buf = rcutils_join_path(root, "governance.p7s", allocator); if (buf) { append(props, DDS::Security::Properties::AccessGovernance, buf); allocator.deallocate(buf, allocator.state); buf = nullptr; - } else { throw std::string("failed to allocate memory for gov_fn"); } + } else { + throw std::string("failed to allocate memory for gov_fn"); + } buf = rcutils_join_path(root, "permissions.p7s", allocator); if (buf) { append(props, DDS::Security::Properties::AccessPermissions, buf); allocator.deallocate(buf, allocator.state); buf = nullptr; - } else { throw std::string("failed to allocate memory for perm_fn"); } + } else { + throw std::string("failed to allocate memory for perm_fn"); + } return true; - } catch (const std::string& e) { + } catch (const std::string &e) { RMW_SET_ERROR_MSG(e.c_str()); - } catch (...) {} + } catch (...) { + } return false; } #endif -void OpenDDSNode::set_default_participant_qos() -{ +void OpenDDSNode::set_default_participant_qos() { DDS::DomainParticipantQos qos; - if (context_.impl->dpf_->get_default_participant_qos(qos) != DDS::RETCODE_OK) { + if (context_.impl->dpf_->get_default_participant_qos(qos) != + DDS::RETCODE_OK) { throw std::runtime_error("get_default_participant_qos failed"); } // since participant name is not part of DDS spec, set node name in user_data - const size_t length = name_.length() + namespace_.length() + strlen("name=;namespace=;") + 1; + const size_t length = + name_.length() + namespace_.length() + strlen("name=;namespace=;") + 1; qos.user_data.value.length(static_cast(length)); - int written = snprintf(reinterpret_cast(qos.user_data.value.get_buffer()), length, - "name=%s;namespace=%s;", name_.c_str(), namespace_.c_str()); + int written = snprintf( + reinterpret_cast(qos.user_data.value.get_buffer()), length, + "name=%s;namespace=%s;", name_.c_str(), namespace_.c_str()); if (written < 0 || written > static_cast(length) - 1) { throw std::runtime_error("failed to set qos.user_data"); } - if (context_.impl->dpf_->set_default_participant_qos(qos) != DDS::RETCODE_OK) { + if (context_.impl->dpf_->set_default_participant_qos(qos) != + DDS::RETCODE_OK) { throw std::runtime_error("set_default_participant_qos failed"); } } -bool OpenDDSNode::configureTransport() -{ +bool OpenDDSNode::configureTransport() { try { static int i = 0; const Guard guard(lock_); - std::string str_domain_id = std::to_string(context_.options.domain_id) + "_" + std::to_string(++i); + std::string str_domain_id = + std::to_string(context_.options.domain_id) + "_" + std::to_string(++i); std::string config_name = "cfg" + str_domain_id; std::string inst_name = "rtps" + str_domain_id; - OpenDDS::DCPS::TransportInst_rch inst = TheTransportRegistry->get_inst(inst_name); + OpenDDS::DCPS::TransportInst_rch inst = + TheTransportRegistry->get_inst(inst_name); if (inst.is_nil()) { inst = TheTransportRegistry->create_inst(inst_name, "rtps_udp"); } - OpenDDS::DCPS::TransportConfig_rch cfg = TheTransportRegistry->get_config(config_name); + OpenDDS::DCPS::TransportConfig_rch cfg = + TheTransportRegistry->get_config(config_name); if (cfg.is_nil()) { cfg = TheTransportRegistry->create_config(config_name); } cfg->instances_.push_back(inst); TheTransportRegistry->bind_config(cfg, dp_.in()); return true; - } catch (const OpenDDS::DCPS::Transport::Exception& e) { - ACE_ERROR_RETURN((LM_ERROR, ACE_TEXT("(%P|%t) ERROR: %C%m\n"), typeid(e).name()), false); - } catch (const CORBA::Exception& e) { - ACE_ERROR_RETURN((LM_ERROR, ACE_TEXT("(%P|%t) ERROR: %C%m\n"), e._info().c_str()), false); + } catch (const OpenDDS::DCPS::Transport::Exception &e) { + ACE_ERROR_RETURN( + (LM_ERROR, ACE_TEXT("(%P|%t) ERROR: %C%m\n"), typeid(e).name()), false); + } catch (const CORBA::Exception &e) { + ACE_ERROR_RETURN( + (LM_ERROR, ACE_TEXT("(%P|%t) ERROR: %C%m\n"), e._info().c_str()), + false); } catch (...) { - ACE_ERROR_RETURN((LM_ERROR, ACE_TEXT("(%P|%t) ERROR: in configureTransport()%m\n")), false); + ACE_ERROR_RETURN( + (LM_ERROR, ACE_TEXT("(%P|%t) ERROR: in configureTransport()%m\n")), + false); } } -bool OpenDDSNode::match(DDS::UserDataQosPolicy & user_data_qos, const std::string & node_name, const std::string & node_namespace) const -{ - uint8_t * buf = user_data_qos.value.get_buffer(); +bool OpenDDSNode::match(DDS::UserDataQosPolicy &user_data_qos, + const std::string &node_name, + const std::string &node_namespace) const { + uint8_t *buf = user_data_qos.value.get_buffer(); if (buf) { std::vector kv(buf, buf + user_data_qos.value.length()); const auto map = rmw::impl::cpp::parse_key_value(kv); @@ -558,11 +600,12 @@ bool OpenDDSNode::match(DDS::UserDataQosPolicy & user_data_qos, const std::strin return false; } -rmw_ret_t OpenDDSNode::get_key(DDS::GUID_t & key, const char * node_name, const char * node_namespace) const -{ +rmw_ret_t OpenDDSNode::get_key(DDS::GUID_t &key, const char *node_name, + const char *node_namespace) const { DDS::DomainParticipantQos qos; auto dds_ret = dp_->get_qos(qos); - if (dds_ret == DDS::RETCODE_OK && match(qos.user_data, node_name, node_namespace)) { + if (dds_ret == DDS::RETCODE_OK && + match(qos.user_data, node_name, node_namespace)) { key = dpi_->get_repoid(dp_->get_instance_handle()); return RMW_RET_OK; } @@ -581,25 +624,22 @@ rmw_ret_t OpenDDSNode::get_key(DDS::GUID_t & key, const char * node_name, const DDS_BuiltinTopicKey_to_GUID(&key, pbtd.key); return RMW_RET_OK; } - } - else { + } else { RMW_SET_ERROR_MSG("unable to fetch discovered participants data."); return RMW_RET_ERROR; } } - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Node name not found: ns='%s', name='%s", - node_namespace, - node_name - ); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Node name not found: ns='%s', name='%s", + node_namespace, node_name); return RMW_RET_NODE_NAME_NON_EXISTENT; } constexpr char SAMPLE_PREFIX[] = "/Sample_"; -rmw_ret_t OpenDDSNode::copy_topic_names_types(rmw_names_and_types_t * nt, - const NameTypeMap & ntm, bool no_demangle, rcutils_allocator_t * allocator) const -{ +rmw_ret_t +OpenDDSNode::copy_topic_names_types(rmw_names_and_types_t *nt, + const NameTypeMap &ntm, bool no_demangle, + rcutils_allocator_t *allocator) const { // Copy data to results handle if (!ntm.empty()) { // Setup string array to store names @@ -610,22 +650,25 @@ rmw_ret_t OpenDDSNode::copy_topic_names_types(rmw_names_and_types_t * nt, // Setup cleanup function, in case of failure below auto fail_cleanup = [&nt]() { if (rmw_names_and_types_fini(nt) != RMW_RET_OK) { - RCUTILS_LOG_ERROR("rmw_names_and_types_fini failed: %s", rmw_get_error_string().str); + RCUTILS_LOG_ERROR("rmw_names_and_types_fini failed: %s", + rmw_get_error_string().str); } }; // Setup demangling functions based on no_demangle option auto demangle_topic = _demangle_if_ros_topic; auto demangle_type = _demangle_if_ros_type; if (no_demangle) { - auto noop = [](const std::string & in) { return in; }; + auto noop = [](const std::string &in) { return in; }; demangle_topic = noop; demangle_type = noop; } - // For each topic, store the name, initialize the string array for types, and store all types + // For each topic, store the name, initialize the string array for types, + // and store all types size_t index = 0; - for (const auto & topic_n_types : ntm) { + for (const auto &topic_n_types : ntm) { // Duplicate and store the topic_name - char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); + char *topic_name = rcutils_strdup( + demangle_topic(topic_n_types.first).c_str(), *allocator); if (!topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for topic name"); fail_cleanup(); @@ -634,7 +677,8 @@ rmw_ret_t OpenDDSNode::copy_topic_names_types(rmw_names_and_types_t * nt, nt->names.data[index] = topic_name; // Setup storage for types { - rcutils_ret_t rcutils_ret = rcutils_string_array_init(&nt->types[index], topic_n_types.second.size(), allocator); + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + &nt->types[index], topic_n_types.second.size(), allocator); if (rcutils_ret != RCUTILS_RET_OK) { RMW_SET_ERROR_MSG(rcutils_get_error_string().str); fail_cleanup(); @@ -643,8 +687,9 @@ rmw_ret_t OpenDDSNode::copy_topic_names_types(rmw_names_and_types_t * nt, } // Duplicate and store each type for the topic size_t type_index = 0; - for (const auto & type : topic_n_types.second) { - char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); + for (const auto &type : topic_n_types.second) { + char *type_name = + rcutils_strdup(demangle_type(type).c_str(), *allocator); if (!type_name) { RMW_SET_ERROR_MSG("failed to allocate memory for type name"); fail_cleanup(); @@ -652,15 +697,17 @@ rmw_ret_t OpenDDSNode::copy_topic_names_types(rmw_names_and_types_t * nt, } nt->types[index].data[type_index] = type_name; ++type_index; - } // for each type + } // for each type ++index; - } // for each topic + } // for each topic } return RMW_RET_OK; } -rmw_ret_t OpenDDSNode::copy_service_names_types(rmw_names_and_types_t * nt, const NameTypeMap & ntm, rcutils_allocator_t * allocator) const -{ +rmw_ret_t +OpenDDSNode::copy_service_names_types(rmw_names_and_types_t *nt, + const NameTypeMap &ntm, + rcutils_allocator_t *allocator) const { if (!ntm.empty()) { // Setup string array to store names rmw_ret_t rmw_ret = rmw_names_and_types_init(nt, ntm.size(), allocator); @@ -670,14 +717,17 @@ rmw_ret_t OpenDDSNode::copy_service_names_types(rmw_names_and_types_t * nt, cons // Setup cleanup function, in case of failure below auto fail_cleanup = [&nt]() { if (rmw_names_and_types_fini(nt) != RMW_RET_OK) { - RCUTILS_LOG_ERROR("rmw_names_and_types_fini failed: %s", rmw_get_error_string().str); + RCUTILS_LOG_ERROR("rmw_names_and_types_fini failed: %s", + rmw_get_error_string().str); } }; - // For each service, store the name, initialize the string array for types, and store all types + // For each service, store the name, initialize the string array for types, + // and store all types size_t index = 0; - for (const auto & service_n_types : ntm) { + for (const auto &service_n_types : ntm) { // Duplicate and store the service_name - char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator); + char *service_name = + rcutils_strdup(service_n_types.first.c_str(), *allocator); if (!service_name) { RMW_SET_ERROR_MSG("failed to allocate memory for service name"); fail_cleanup(); @@ -686,7 +736,8 @@ rmw_ret_t OpenDDSNode::copy_service_names_types(rmw_names_and_types_t * nt, cons nt->names.data[index] = service_name; // Setup storage for types { - rcutils_ret_t rcutils_ret = rcutils_string_array_init(&nt->types[index], service_n_types.second.size(), allocator); + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + &nt->types[index], service_n_types.second.size(), allocator); if (rcutils_ret != RCUTILS_RET_OK) { RMW_SET_ERROR_MSG(rcutils_get_error_string().str); fail_cleanup(); @@ -695,15 +746,17 @@ rmw_ret_t OpenDDSNode::copy_service_names_types(rmw_names_and_types_t * nt, cons } // Duplicate and store each type for the service size_t type_index = 0; - for (const auto & type : service_n_types.second) { - // Strip the SAMPLE_PREFIX if it is found (e.g. from services using OpenSplice typesupport). - // It may not be found if services are detected using other typesupports. + for (const auto &type : service_n_types.second) { + // Strip the SAMPLE_PREFIX if it is found (e.g. from services using + // OpenSplice typesupport). It may not be found if services are detected + // using other typesupports. size_t n = type.find(SAMPLE_PREFIX); std::string stripped_type = type; if (std::string::npos != n) { - stripped_type = type.substr(0, n + 1) + type.substr(n + strlen(SAMPLE_PREFIX)); + stripped_type = + type.substr(0, n + 1) + type.substr(n + strlen(SAMPLE_PREFIX)); } - char * type_name = rcutils_strdup(stripped_type.c_str(), *allocator); + char *type_name = rcutils_strdup(stripped_type.c_str(), *allocator); if (!type_name) { RMW_SET_ERROR_MSG("failed to allocate memory for type name"); fail_cleanup(); @@ -711,9 +764,9 @@ rmw_ret_t OpenDDSNode::copy_service_names_types(rmw_names_and_types_t * nt, cons } nt->types[index].data[type_index] = type_name; ++type_index; - } // for each type + } // for each type ++index; - } // for each service + } // for each service } return RMW_RET_OK; } diff --git a/rmw_opendds_cpp/src/init.cpp b/rmw_opendds_cpp/src/init.cpp index 6d66a3f..fd1edaf 100644 --- a/rmw_opendds_cpp/src/init.cpp +++ b/rmw_opendds_cpp/src/init.cpp @@ -12,36 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include -#include #include +#include #include -rmw_context_impl_t::rmw_context_impl_t() : dpf_(TheParticipantFactory) -{ +rmw_context_impl_t::rmw_context_impl_s() : dpf_(TheParticipantFactory) { if (dpf_) { - TheServiceParticipant->set_default_discovery(OpenDDS::DCPS::Discovery::DEFAULT_RTPS); + TheServiceParticipant->set_default_discovery( + OpenDDS::DCPS::Discovery::DEFAULT_RTPS); } else { - const char* msg = "failed to get participant factory"; + const char *msg = "failed to get participant factory"; RMW_SET_ERROR_MSG(msg); throw std::runtime_error(msg); } } -rmw_context_impl_t::~rmw_context_impl_t() -{ +rmw_context_impl_t::~rmw_context_impl_s() { if (dpf_) { TheServiceParticipant->shutdown(); dpf_ = nullptr; } } -rmw_ret_t -init(rmw_context_t& context) -{ +rmw_ret_t init(rmw_context_t &context) { context.impl = RmwAllocateFree::create(); if (context.impl) { return RMW_RET_OK; @@ -53,29 +50,23 @@ init(rmw_context_t& context) // See rmw/init.h: rmw_ret_t rmw_shutdown(rmw_context_t * context) // "If context has been already invalidated (`rmw_shutdown()` was called on it), // then this function is a no-op and `RMW_RET_OK` is returned." -rmw_ret_t -shutdown(rmw_context_t& context) -{ +rmw_ret_t shutdown(rmw_context_t &context) { if (context.impl) { RmwAllocateFree::destroy(context.impl); } return RMW_RET_OK; } -bool is_zero_initialized(const rmw_init_options_t * opt) -{ - return opt && opt->instance_id == 0 - && !(opt->implementation_identifier) - && opt->domain_id == RMW_DEFAULT_DOMAIN_ID - && opt->security_options.enforce_security == 0 && !(opt->security_options.security_root_path) - && opt->localhost_only == RMW_LOCALHOST_ONLY_DEFAULT - && !(opt->enclave) - && !(opt->impl); +bool is_zero_initialized(const rmw_init_options_t *opt) { + return opt && opt->instance_id == 0 && !(opt->implementation_identifier) && + opt->domain_id == RMW_DEFAULT_DOMAIN_ID && + opt->security_options.enforce_security == 0 && + !(opt->security_options.security_root_path) && + opt->localhost_only == RMW_LOCALHOST_ONLY_DEFAULT && !(opt->enclave) && + !(opt->impl); } -bool is_zero_initialized(const rmw_context_t * ctx) -{ - return ctx && ctx->instance_id == 0 - && !(ctx->implementation_identifier) - && !(ctx->impl); +bool is_zero_initialized(const rmw_context_t *ctx) { + return ctx && ctx->instance_id == 0 && !(ctx->implementation_identifier) && + !(ctx->impl); } diff --git a/rmw_opendds_cpp/src/qos.cpp b/rmw_opendds_cpp/src/qos.cpp index 601146b..7ae6806 100644 --- a/rmw_opendds_cpp/src/qos.cpp +++ b/rmw_opendds_cpp/src/qos.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include bool @@ -61,3 +63,20 @@ dds_qos_policy_to_rmw_qos_policy(DDS::QosPolicyId_t policy_id) } } +rmw_ret_t +rmw_qos_profile_check_compatible ( + const rmw_qos_profile_t publisher_profile, + const rmw_qos_profile_t subscriber_profile, + rmw_qos_compatibility_type_t * compatibility, + char * reason, + size_t reason_size) +{ + (void) publisher_profile; + (void) subscriber_profile; + (void) compatibility; + (void) reason; + (void) reason_size; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} diff --git a/rmw_opendds_cpp/src/rmw_client.cpp b/rmw_opendds_cpp/src/rmw_client.cpp index 06d1d51..0de74cd 100644 --- a/rmw_opendds_cpp/src/rmw_client.cpp +++ b/rmw_opendds_cpp/src/rmw_client.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -164,4 +165,67 @@ rmw_take_response( *taken = dds_client->take(request_header, ros_response); return RMW_RET_OK; } + +rmw_ret_t +rmw_client_set_on_new_response_callback( + rmw_client_t * client, + rmw_event_callback_t callback, + const void * user_data) +{ + (void) client; + (void) callback; + (void) user_data; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_client_request_publisher_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + (void) client; + (void) qos; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_client_response_subscriber_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + (void) client; + (void) qos; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_client_response_publisher_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + (void) client; + (void) qos; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_client_response_subscription_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + (void) client; + (void) qos; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + } // extern "C" diff --git a/rmw_opendds_cpp/src/rmw_event.cpp b/rmw_opendds_cpp/src/rmw_event.cpp index 2103df2..c25a52c 100644 --- a/rmw_opendds_cpp/src/rmw_event.cpp +++ b/rmw_opendds_cpp/src/rmw_event.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include @@ -59,4 +61,19 @@ extern "C" event_info, taken); } + + rmw_ret_t + rmw_event_set_callback ( + rmw_event_t * event, + rmw_event_callback_t callback, + const void * user_data) + { + (void) event; + (void) callback; + (void) user_data; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; + } + } // extern "C" diff --git a/rmw_opendds_cpp/src/rmw_features.cpp b/rmw_opendds_cpp/src/rmw_features.cpp new file mode 100644 index 0000000..68017c0 --- /dev/null +++ b/rmw_opendds_cpp/src/rmw_features.cpp @@ -0,0 +1,7 @@ +#include + +bool +rmw_feature_supported (rmw_feature_t feature) { + (void) feature; + return false; +} diff --git a/rmw_opendds_cpp/src/rmw_get_network_flow_endpoints.cpp b/rmw_opendds_cpp/src/rmw_get_network_flow_endpoints.cpp new file mode 100644 index 0000000..118bd39 --- /dev/null +++ b/rmw_opendds_cpp/src/rmw_get_network_flow_endpoints.cpp @@ -0,0 +1,35 @@ + +#include +#include +#include +#include +#include +#include + +rmw_ret_t +rmw_publisher_get_network_flow_endpoints ( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoints) +{ + (void) publisher; + (void) allocator; + (void) network_flow_endpoints; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_subscription_get_network_flow_endpoints ( + const rmw_subscription_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoints) +{ + (void) publisher; + (void) allocator; + (void) network_flow_endpoints; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} diff --git a/rmw_opendds_cpp/src/rmw_node.cpp b/rmw_opendds_cpp/src/rmw_node.cpp index ecb5cd1..f609ff5 100644 --- a/rmw_opendds_cpp/src/rmw_node.cpp +++ b/rmw_opendds_cpp/src/rmw_node.cpp @@ -48,12 +48,8 @@ rmw_node_t * rmw_create_node( rmw_context_t * context, const char * name, - const char * namespace_, - size_t domain_id, - bool localhost_only) + const char * namespace_) { - ACE_UNUSED_ARG(domain_id); - ACE_UNUSED_ARG(localhost_only); RMW_CHECK_FOR_NULL_WITH_MSG(context, "context is null", nullptr); if (!check_impl_id(context->implementation_identifier)) { return nullptr; diff --git a/rmw_opendds_cpp/src/rmw_publisher.cpp b/rmw_opendds_cpp/src/rmw_publisher.cpp index bec8f73..2ab58b1 100644 --- a/rmw_opendds_cpp/src/rmw_publisher.cpp +++ b/rmw_opendds_cpp/src/rmw_publisher.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -49,6 +50,18 @@ rmw_init_publisher_allocation( return RMW_RET_ERROR; } +rmw_ret_t +rmw_publisher_wait_for_all_acked( + const rmw_publisher_t * publisher, + rmw_time_t wait_timeout) +{ + (void) publisher; + (void) wait_timeout; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + rmw_ret_t rmw_fini_publisher_allocation(rmw_publisher_allocation_t * allocation) { diff --git a/rmw_opendds_cpp/src/rmw_server.cpp b/rmw_opendds_cpp/src/rmw_server.cpp index 3f205e4..c248244 100644 --- a/rmw_opendds_cpp/src/rmw_server.cpp +++ b/rmw_opendds_cpp/src/rmw_server.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include @@ -95,6 +97,44 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) return RMW_RET_ERROR; } +rmw_ret_t +rmw_service_set_on_new_request_callback( + rmw_service_t * service, + rmw_event_callback_t callback, + const void * user_data) +{ + (void) service; + (void) callback; + (void) user_data; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_service_request_subscription_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + (void) service; + (void) qos; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_service_response_publisher_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + (void) service; + (void) qos; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + rmw_ret_t rmw_take_request( const rmw_service_t * service, diff --git a/rmw_opendds_cpp/src/rmw_subscription.cpp b/rmw_opendds_cpp/src/rmw_subscription.cpp index f07721f..d3c1804 100644 --- a/rmw_opendds_cpp/src/rmw_subscription.cpp +++ b/rmw_opendds_cpp/src/rmw_subscription.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include +#include #include #include #include @@ -137,6 +141,46 @@ rmw_subscription_count_matched_publishers( return RMW_RET_ERROR; } +rmw_ret_t +rmw_subscription_set_content_filter ( + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options) +{ + (void) subscription; + (void) options; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_subscription_get_content_filter ( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options) +{ + (void) subscription; + (void) allocator; + (void) options; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_subscription_set_on_new_message_callback( + rmw_subscription_t * subscription, + rmw_event_callback_t callback, + const void * user_data) +{ + (void) subscription; + (void) callback; + (void) user_data; + // Unused in current implementation. + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; +} + rmw_ret_t rmw_subscription_get_actual_qos( const rmw_subscription_t * subscription, From bb5681ad1dfaf399ce79ec7890b2f13436fd0d58 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 29 Mar 2024 13:36:30 +0000 Subject: [PATCH 2/3] implementation of required API some debug code to help test --- .../include/rmw_opendds_cpp/DDSServer.hpp | 4 +++ rmw_opendds_cpp/src/DDSServer.cpp | 20 ++++++++++++++ rmw_opendds_cpp/src/init.cpp | 2 ++ rmw_opendds_cpp/src/rmw_server.cpp | 26 ++++++++++++------- 4 files changed, 43 insertions(+), 9 deletions(-) diff --git a/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp b/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp index 9160d15..b28084c 100644 --- a/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp +++ b/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp @@ -15,6 +15,7 @@ #ifndef RMW_OPENDDS_CPP__DDSSERVER_HPP_ #define RMW_OPENDDS_CPP__DDSSERVER_HPP_ +#include #include #include #include @@ -34,6 +35,9 @@ class DDSServer void * replier() const { return replier_; } DDS::ReadCondition_var read_condition() const { return read_condition_; } + rmw_ret_t subscription_get_actual_qos(rmw_qos_profile_t* qos); + rmw_ret_t publisher_get_actual_qos(rmw_qos_profile_t* qos); + private: friend Raf; DDSServer(const rosidl_service_type_support_t * ts, const char * service_name, diff --git a/rmw_opendds_cpp/src/DDSServer.cpp b/rmw_opendds_cpp/src/DDSServer.cpp index 20cd7bc..cbf8139 100644 --- a/rmw_opendds_cpp/src/DDSServer.cpp +++ b/rmw_opendds_cpp/src/DDSServer.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rmw_opendds_cpp/qos.hpp" +#include +#include #include #include @@ -75,6 +78,23 @@ bool DDSServer::remove_from(OpenDDSNode * dds_node) return false; } +rmw_ret_t DDSServer::subscription_get_actual_qos(rmw_qos_profile_t* qos) { + DDS::DataReaderQos dds_qos; + reader_->get_qos(dds_qos); + + dds_qos_to_rmw_qos(dds_qos, *qos); + return RMW_RET_OK; +} + +rmw_ret_t DDSServer::publisher_get_actual_qos(rmw_qos_profile_t* qos) { + DDS::DataWriterQos dds_qos; + writer_->get_qos(dds_qos); + + dds_qos_to_rmw_qos(dds_qos, *qos); + return RMW_RET_OK; +} + + DDSServer::DDSServer(const rosidl_service_type_support_t * ts , const char * service_name , const rmw_qos_profile_t * rmw_qos diff --git a/rmw_opendds_cpp/src/init.cpp b/rmw_opendds_cpp/src/init.cpp index fd1edaf..975bee0 100644 --- a/rmw_opendds_cpp/src/init.cpp +++ b/rmw_opendds_cpp/src/init.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include @@ -24,6 +25,7 @@ rmw_context_impl_t::rmw_context_impl_s() : dpf_(TheParticipantFactory) { if (dpf_) { TheServiceParticipant->set_default_discovery( OpenDDS::DCPS::Discovery::DEFAULT_RTPS); + OpenDDS::DCPS::log_level = OpenDDS::DCPS::LogLevel::Debug; } else { const char *msg = "failed to get participant factory"; RMW_SET_ERROR_MSG(msg); diff --git a/rmw_opendds_cpp/src/rmw_server.cpp b/rmw_opendds_cpp/src/rmw_server.cpp index c248244..9f2c877 100644 --- a/rmw_opendds_cpp/src/rmw_server.cpp +++ b/rmw_opendds_cpp/src/rmw_server.cpp @@ -116,11 +116,15 @@ rmw_service_request_subscription_get_actual_qos( const rmw_service_t * service, rmw_qos_profile_t * qos) { - (void) service; - (void) qos; - // Unused in current implementation. - RMW_SET_ERROR_MSG("unimplemented"); - return RMW_RET_ERROR; + auto dds_server = DDSServer::from(service); + if (!dds_server) + return RMW_RET_INVALID_ARGUMENT; + + auto ret = dds_server->subscription_get_actual_qos(qos); + if (ret != RMW_RET_OK) + return ret; + + return RMW_RET_OK; } rmw_ret_t @@ -128,10 +132,14 @@ rmw_service_response_publisher_get_actual_qos( const rmw_service_t * service, rmw_qos_profile_t * qos) { - (void) service; - (void) qos; - // Unused in current implementation. - RMW_SET_ERROR_MSG("unimplemented"); + auto dds_server = DDSServer::from(service); + if (!dds_server) + return RMW_RET_INVALID_ARGUMENT; + + auto ret = dds_server->publisher_get_actual_qos(qos); + if (ret != RMW_RET_OK) + return ret; + return RMW_RET_ERROR; } From fcf891de4eca8d90692bf9484c986f0e4e6675f1 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Sat, 6 Apr 2024 16:11:58 -0400 Subject: [PATCH 3/3] uncomplicated some small funcs --- .../include/rmw_opendds_cpp/DDSServer.hpp | 4 ++-- rmw_opendds_cpp/package.xml | 2 +- rmw_opendds_cpp/src/DDSServer.cpp | 6 ++---- rmw_opendds_cpp/src/rmw_server.cpp | 12 ++++-------- 4 files changed, 9 insertions(+), 15 deletions(-) diff --git a/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp b/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp index b28084c..f350cac 100644 --- a/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp +++ b/rmw_opendds_cpp/include/rmw_opendds_cpp/DDSServer.hpp @@ -35,8 +35,8 @@ class DDSServer void * replier() const { return replier_; } DDS::ReadCondition_var read_condition() const { return read_condition_; } - rmw_ret_t subscription_get_actual_qos(rmw_qos_profile_t* qos); - rmw_ret_t publisher_get_actual_qos(rmw_qos_profile_t* qos); + void subscription_get_actual_qos(rmw_qos_profile_t* qos); + void publisher_get_actual_qos(rmw_qos_profile_t* qos); private: friend Raf; diff --git a/rmw_opendds_cpp/package.xml b/rmw_opendds_cpp/package.xml index fcedc57..08842ce 100644 --- a/rmw_opendds_cpp/package.xml +++ b/rmw_opendds_cpp/package.xml @@ -13,7 +13,7 @@ ament_cmake rosidl_cmake - + opendds_cmake_module rcutils rmw diff --git a/rmw_opendds_cpp/src/DDSServer.cpp b/rmw_opendds_cpp/src/DDSServer.cpp index cbf8139..3298a6d 100644 --- a/rmw_opendds_cpp/src/DDSServer.cpp +++ b/rmw_opendds_cpp/src/DDSServer.cpp @@ -78,20 +78,18 @@ bool DDSServer::remove_from(OpenDDSNode * dds_node) return false; } -rmw_ret_t DDSServer::subscription_get_actual_qos(rmw_qos_profile_t* qos) { +void DDSServer::subscription_get_actual_qos(rmw_qos_profile_t* qos) { DDS::DataReaderQos dds_qos; reader_->get_qos(dds_qos); dds_qos_to_rmw_qos(dds_qos, *qos); - return RMW_RET_OK; } -rmw_ret_t DDSServer::publisher_get_actual_qos(rmw_qos_profile_t* qos) { +void DDSServer::publisher_get_actual_qos(rmw_qos_profile_t* qos) { DDS::DataWriterQos dds_qos; writer_->get_qos(dds_qos); dds_qos_to_rmw_qos(dds_qos, *qos); - return RMW_RET_OK; } diff --git a/rmw_opendds_cpp/src/rmw_server.cpp b/rmw_opendds_cpp/src/rmw_server.cpp index 9f2c877..513c45c 100644 --- a/rmw_opendds_cpp/src/rmw_server.cpp +++ b/rmw_opendds_cpp/src/rmw_server.cpp @@ -120,10 +120,8 @@ rmw_service_request_subscription_get_actual_qos( if (!dds_server) return RMW_RET_INVALID_ARGUMENT; - auto ret = dds_server->subscription_get_actual_qos(qos); - if (ret != RMW_RET_OK) - return ret; - + dds_server->subscription_get_actual_qos(qos); + return RMW_RET_OK; } @@ -136,11 +134,9 @@ rmw_service_response_publisher_get_actual_qos( if (!dds_server) return RMW_RET_INVALID_ARGUMENT; - auto ret = dds_server->publisher_get_actual_qos(qos); - if (ret != RMW_RET_OK) - return ret; + dds_server->publisher_get_actual_qos(qos); - return RMW_RET_ERROR; + return RMW_RET_OK; } rmw_ret_t