From 67212924b554ac8c0a583da72622814c121bde37 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 17 Oct 2019 09:57:39 -0700 Subject: [PATCH 01/23] Simpled test for topic_cache Signed-off-by: Jaison Titus --- rmw_fastrtps_shared_cpp/test/CMakeLists.txt | 7 ++ .../test/test_topic_cache.cpp | 88 +++++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp diff --git a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt index 8abf78e36..868a5bcbf 100644 --- a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt @@ -1,6 +1,13 @@ find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_dds_attributes_to_rmw_qos test_dds_attributes_to_rmw_qos.cpp) if(TARGET test_dds_attributes_to_rmw_qos) ament_target_dependencies(test_dds_attributes_to_rmw_qos) target_link_libraries(test_dds_attributes_to_rmw_qos ${PROJECT_NAME}) endif() + +ament_add_gtest(test_topic_cache test_topic_cache.cpp) +if(TARGET test_topic_cache) + ament_target_dependencies(test_topic_cache) + target_link_libraries(test_topic_cache ${PROJECT_NAME}) +endif() diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp new file mode 100644 index 000000000..c4d21eaf9 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -0,0 +1,88 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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 "gtest/gtest.h" + +#include "fastrtps/qos/ReaderQos.h" +#include "fastrtps/qos/WriterQos.h" +#include "fastrtps/rtps/common/InstanceHandle.h" + +#include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" + +using eprosima::fastrtps::WriterQos; +using eprosima::fastrtps::ReaderQos; +using eprosima::fastrtps::rtps::GUID_t; +using eprosima::fastrtps::rtps::GuidPrefix_t; +using eprosima::fastrtps::rtps::InstanceHandle_t; + +TEST(TopicCacheTest, test_topic_cache_add_topic) { + // Create an instance handler + const auto guid = GUID_t(GuidPrefix_t(), 1); + InstanceHandle_t instance_handler = InstanceHandle_t(); + instance_handler = guid; + + // Create an instance of TopicCache + auto topic_cache = TopicCache(); + // Add Topic + bool did_add = topic_cache.addTopic(instance_handler, "TestTopic", "TestType"); + // Verify that the returned value was true + EXPECT_TRUE(did_add); + + // Verify that there exists a value of the guid in the map + const auto & it = topic_cache.getTopicToTypes().find("TestTopic"); + ASSERT_FALSE(it == topic_cache.getTopicToTypes().end()); + + // Verify that the object returned from the map is indeed the one expected + const auto topic_types = it->second; + // Verify that this vector only has one element + EXPECT_EQ(topic_types.size(), 1u); + // Verify the topic topic is present in the vector + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "TestType") != topic_types.end()); +} + +TEST(TopicCacheTest, test_topic_cache_remove_policy_element_exists) { + // Create an instance handler + const auto guid = GUID_t(GuidPrefix_t(), 2); + InstanceHandle_t instance_handler = InstanceHandle_t(); + instance_handler = guid; + + // Create an instance of TopicCache + auto topic_cache = TopicCache(); + // add topic + topic_cache.addTopic(instance_handler, "TestTopic", "TestType"); + // remove topic + auto did_remove = topic_cache.removeTopic(instance_handler, "TestTopic", "TestType"); + // Assert that the return was true + ASSERT_TRUE(did_remove); + + // Verify that there does not exist a value for the guid in the map + const auto & it = topic_cache.getTopicToTypes().find("TestTopic"); + ASSERT_TRUE(it == topic_cache.getTopicToTypes().end()); +} + +TEST(TopicCacheTest, test_topic_cache_remove_policy_element_does_not_exist) { + // Create an instance handler + const auto guid = GUID_t(GuidPrefix_t(), 3); + InstanceHandle_t instance_handler = InstanceHandle_t(); + instance_handler = guid; + + // Create an instance of TopicCache + auto topic_cache = TopicCache(); + // add topic + topic_cache.addTopic(instance_handler, "TestTopic", "TestType"); + // Assert that the return was false + auto const did_remove = topic_cache.removeTopic(instance_handler, "NewTestTopic", "TestType"); + ASSERT_FALSE(did_remove); +} From 944e1f3f9a6b7e75134add871e9b85dcad6a1751 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 18 Oct 2019 14:32:26 -0700 Subject: [PATCH 02/23] Moved qos_converter to include/ since it needs to be accessible to topic_cache.hpp Signed-off-by: Jaison Titus --- .../rmw_fastrtps_shared_cpp}/qos_converter.hpp | 8 ++++---- rmw_fastrtps_shared_cpp/src/qos.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) rename rmw_fastrtps_shared_cpp/{src => include/rmw_fastrtps_shared_cpp}/qos_converter.hpp (92%) diff --git a/rmw_fastrtps_shared_cpp/src/qos_converter.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp similarity index 92% rename from rmw_fastrtps_shared_cpp/src/qos_converter.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp index 6f7460cd1..c78ddb6c9 100644 --- a/rmw_fastrtps_shared_cpp/src/qos_converter.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp @@ -1,4 +1,4 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,8 +13,8 @@ // limitations under the License. -#ifndef QOS_CONVERTER_HPP_ -#define QOS_CONVERTER_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__QOS_CONVERTER_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__QOS_CONVERTER_HPP_ #include "rmw/types.h" @@ -83,4 +83,4 @@ dds_qos_to_rmw_qos( qos->liveliness_lease_duration.nsec = dds_qos.m_liveliness.lease_duration.nanosec; } -#endif // QOS_CONVERTER_HPP_ +#endif // RMW_FASTRTPS_SHARED_CPP__QOS_CONVERTER_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index beaae41fb..72aaece31 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -14,7 +14,7 @@ #include -#include "qos_converter.hpp" +#include "rmw_fastrtps_shared_cpp/qos_converter.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "fastrtps/attributes/PublisherAttributes.h" From e7a4a7e55980b0344c970fd1b5226152a73a9fc0 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 18 Oct 2019 14:38:40 -0700 Subject: [PATCH 03/23] - Modified topic_cache to include qos policies - Modified call to topic_cache.addTopic at custom_participant_info.hpp - Wrote tests for topic_cache Signed-off-by: Jaison Titus --- .../custom_participant_info.hpp | 7 +- .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 81 ++++-- .../test/test_topic_cache.cpp | 252 ++++++++++++++---- 3 files changed, 273 insertions(+), 67 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index eb6fbc4fe..b66510e37 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -169,8 +169,11 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener { std::lock_guard guard(topic_cache.getMutex()); if (is_alive) { - trigger = topic_cache().addTopic(proxyData.RTPSParticipantKey(), - proxyData.topicName().to_string(), proxyData.typeName().to_string()); + trigger = topic_cache().addTopic( + proxyData.RTPSParticipantKey(), + proxyData.topicName().to_string(), + proxyData.typeName().to_string(), + proxyData.m_qos); } else { trigger = topic_cache().removeTopic(proxyData.RTPSParticipantKey(), proxyData.topicName().to_string(), proxyData.typeName().to_string()); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index c5604ee93..ff37eb6a7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include "fastrtps/participant/Participant.h" #include "fastrtps/rtps/common/Guid.h" #include "fastrtps/rtps/common/InstanceHandle.h" +#include "qos_converter.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" @@ -39,19 +41,20 @@ typedef eprosima::fastrtps::rtps::GUID_t GUID_t; class TopicCache { private: - typedef std::map>> ParticipantTopicMap; typedef std::unordered_map> TopicToTypes; + typedef std::map ParticipantTopicMap; + typedef std::vector> TopicData; + typedef std::unordered_map TopicNameToTopicData; /** - * Map of topic names to a vector of types that topic may use. + * Map of topic names to TopicData. Where topic data is vector of tuples containing + * participant GUID, topic type and the qos policy of the respective participant. * Topics here are represented as one to many, DDS XTypes 1.2 * specifies application code 'generally' uses a 1-1 relationship. * However, generic services such as logger and monitor, can discover * multiple types on the same topic. - * */ - TopicToTypes topic_to_types_; + TopicNameToTopicData topic_name_to_topic_data_; /** * Map of participant GUIDS to a set of topic-type. @@ -59,11 +62,27 @@ class TopicCache ParticipantTopicMap participant_to_topics_; /** - * Helper function to initialize a topic vector. + * Helper function to initialize an empty TopicData for a topic name. * - * @param topic_name + * @param topic_name the topic name for which the TopicNameToTopicData map should be initialised. + * @param topic_name_to_topic_data the map to initialise. */ - void initializeTopic(const std::string & topic_name, TopicToTypes & topic_to_types) + void initializeTopicDataMap( + const std::string & topic_name, + TopicNameToTopicData & topic_name_to_topic_data) + { + if (topic_name_to_topic_data.find(topic_name) == topic_name_to_topic_data.end()) { + topic_name_to_topic_data[topic_name] = TopicData(); + } + } + + /** + * Helper function to initialize a topic vector. + * + * @param topic_name the topic name for which the TopicToTypes map should be initialised. + * @param topic_to_types the map to initialise. + */ + void initializeTopicTypesMap(const std::string & topic_name, TopicToTypes & topic_to_types) { if (topic_to_types.find(topic_name) == topic_to_types.end()) { topic_to_types[topic_name] = std::vector(); @@ -89,9 +108,16 @@ class TopicCache /** * @return a map of topic name to the vector of topic types used. */ - const TopicToTypes & getTopicToTypes() const + const TopicToTypes getTopicToTypes() const { - return topic_to_types_; + TopicToTypes topic_to_types; + for (const auto & it : topic_name_to_topic_data_) { + topic_to_types[it.first] = std::vector(); + for (const auto & mit : it.second) { + topic_to_types[it.first].push_back(std::get<1>(mit)); + } + } + return topic_to_types; } /** @@ -102,6 +128,14 @@ class TopicCache return participant_to_topics_; } + /** + * @return a map of topic name to a vector of GUID_t, type name and qos profile tuple. + */ + const TopicNameToTopicData & getTopicNameToTopicData() const + { + return topic_name_to_topic_data_; + } + /** * Add a topic based on discovery. * @@ -110,15 +144,17 @@ class TopicCache * @param type_name * @return true if a change has been recorded */ + template bool addTopic( const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, const std::string & topic_name, - const std::string & type_name) + const std::string & type_name, + const T & dds_qos) { - initializeTopic(topic_name, topic_to_types_); + initializeTopicDataMap(topic_name, topic_name_to_topic_data_); auto guid = iHandle2GUID(rtpsParticipantKey); initializeParticipantMap(participant_to_topics_, guid); - initializeTopic(topic_name, participant_to_topics_[guid]); + initializeTopicTypesMap(topic_name, participant_to_topics_[guid]); if (rcutils_logging_logger_is_enabled_for("rmw_fastrtps_shared_cpp", RCUTILS_LOG_SEVERITY_DEBUG)) { @@ -129,7 +165,9 @@ class TopicCache "Adding topic '%s' with type '%s' for node '%s'", topic_name.c_str(), type_name.c_str(), guid_stream.str().c_str()); } - topic_to_types_[topic_name].push_back(type_name); + auto qos_profile = rmw_qos_profile_t(); + dds_qos_to_rmw_qos(dds_qos, &qos_profile); + topic_name_to_topic_data_[topic_name].push_back(std::make_tuple(guid, type_name, qos_profile)); participant_to_topics_[guid][topic_name].push_back(type_name); return true; } @@ -147,22 +185,26 @@ class TopicCache const std::string & topic_name, const std::string & type_name) { - if (topic_to_types_.find(topic_name) == topic_to_types_.end()) { + if (topic_name_to_topic_data_.find(topic_name) == topic_name_to_topic_data_.end()) { RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_shared_cpp", "unexpected removal on topic '%s' with type '%s'", topic_name.c_str(), type_name.c_str()); return false; } + auto guid = iHandle2GUID(rtpsParticipantKey); { - auto & type_vec = topic_to_types_[topic_name]; - type_vec.erase(std::find(type_vec.begin(), type_vec.end(), type_name)); + auto & type_vec = topic_name_to_topic_data_[topic_name]; + type_vec.erase(std::find_if(type_vec.begin(), type_vec.end(), + [type_name, guid](const auto & topic_info) { + return type_name.compare(std::get<1>(topic_info)) == 0 && + guid == std::get<0>(topic_info); + })); if (type_vec.empty()) { - topic_to_types_.erase(topic_name); + topic_name_to_topic_data_.erase(topic_name); } } - auto guid = iHandle2GUID(rtpsParticipantKey); auto guid_topics_pair = participant_to_topics_.find(guid); if (guid_topics_pair != participant_to_topics_.end() && guid_topics_pair->second.find(topic_name) != guid_topics_pair->second.end()) @@ -180,6 +222,7 @@ class TopicCache "rmw_fastrtps_shared_cpp", "Unable to remove topic, does not exist '%s' with type '%s'", topic_name.c_str(), type_name.c_str()); + return false; } return true; } diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index c4d21eaf9..c49bfcda8 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -11,15 +11,19 @@ // 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 +#include +#include +#include #include "gtest/gtest.h" +#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" + +#include "fastrtps/rtps/common/InstanceHandle.h" #include "fastrtps/qos/ReaderQos.h" #include "fastrtps/qos/WriterQos.h" -#include "fastrtps/rtps/common/InstanceHandle.h" - #include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" using eprosima::fastrtps::WriterQos; using eprosima::fastrtps::ReaderQos; @@ -27,62 +31,218 @@ using eprosima::fastrtps::rtps::GUID_t; using eprosima::fastrtps::rtps::GuidPrefix_t; using eprosima::fastrtps::rtps::InstanceHandle_t; -TEST(TopicCacheTest, test_topic_cache_add_topic) { - // Create an instance handler - const auto guid = GUID_t(GuidPrefix_t(), 1); - InstanceHandle_t instance_handler = InstanceHandle_t(); - instance_handler = guid; +class TopicCacheTestFixture : public ::testing::Test +{ +public: + TopicCache topic_cache; + WriterQos qos[2]; + rmw_qos_profile_t rmw_qos[2]; + InstanceHandle_t instance_handler[2]; + GUID_t guid[2]; + void SetUp() + { + // Create an instance handlers + for (int i = 0; i < 2; i++) { + guid[i] = GUID_t(GuidPrefix_t(), i + 1); + instance_handler[i] = guid[i]; + } - // Create an instance of TopicCache - auto topic_cache = TopicCache(); - // Add Topic - bool did_add = topic_cache.addTopic(instance_handler, "TestTopic", "TestType"); - // Verify that the returned value was true - EXPECT_TRUE(did_add); + // DDS qos + qos[0].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; + qos[0].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + qos[0].m_deadline.period = {123, 5678}; + qos[0].m_lifespan.duration = {190, 1234}; + qos[0].m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; + qos[0].m_liveliness.lease_duration = {80, 5555555}; - // Verify that there exists a value of the guid in the map - const auto & it = topic_cache.getTopicToTypes().find("TestTopic"); - ASSERT_FALSE(it == topic_cache.getTopicToTypes().end()); + // equivalent rmq qos + rmw_qos[0].durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + rmw_qos[0].reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + rmw_qos[0].liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + rmw_qos[0].liveliness_lease_duration.sec = 80u; + rmw_qos[0].liveliness_lease_duration.nsec = 5555555u; + rmw_qos[0].deadline.sec = 123u; + rmw_qos[0].deadline.nsec = 5678u; + rmw_qos[0].lifespan.sec = 190u; + rmw_qos[0].lifespan.nsec = 1234u; + // DDS qos + qos[1].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; + qos[1].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + qos[1].m_deadline.period = {12, 1234}; + qos[1].m_lifespan.duration = {19, 5432}; + qos[1].m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; + qos[1].m_liveliness.lease_duration = {8, 78901234}; + + // equivalent rmq qos + rmw_qos[1].durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + rmw_qos[1].reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + rmw_qos[1].liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + rmw_qos[1].liveliness_lease_duration.sec = 8u; + rmw_qos[1].liveliness_lease_duration.nsec = 78901234u; + rmw_qos[1].deadline.sec = 12u; + rmw_qos[1].deadline.nsec = 1234u; + rmw_qos[1].lifespan.sec = 19u; + rmw_qos[1].lifespan.nsec = 5432u; + + // Add some topics + topic_cache.addTopic(instance_handler[0], "topic1", "type1", qos[0]); + topic_cache.addTopic(instance_handler[0], "topic2", "type2", qos[0]); + topic_cache.addTopic(instance_handler[1], "topic1", "type1", qos[1]); + topic_cache.addTopic(instance_handler[1], "topic2", "type1", qos[1]); + } +}; + +TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_types) +{ + const auto & topic_type_map = this->topic_cache.getTopicToTypes(); + + // topic1 + const auto & it = topic_type_map.find("topic1"); + ASSERT_TRUE(it != topic_type_map.end()); // Verify that the object returned from the map is indeed the one expected - const auto topic_types = it->second; - // Verify that this vector only has one element - EXPECT_EQ(topic_types.size(), 1u); - // Verify the topic topic is present in the vector - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "TestType") != topic_types.end()); + auto topic_types = it->second; + // Verify that there are two entries for type1 + EXPECT_EQ(std::count(topic_types.begin(), topic_types.end(), "type1"), 2); + EXPECT_EQ(topic_types.at(0), "type1"); + + // topic2 + const auto & it2 = topic_type_map.find("topic2"); + ASSERT_TRUE(it2 != topic_type_map.end()); + // Verify that the object returned from the map is indeed the one expected + topic_types = it2->second; + // Verify that there are two entries for type1 + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); } -TEST(TopicCacheTest, test_topic_cache_remove_policy_element_exists) { - // Create an instance handler - const auto guid = GUID_t(GuidPrefix_t(), 2); - InstanceHandle_t instance_handler = InstanceHandle_t(); - instance_handler = guid; +TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) +{ + const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); - // Create an instance of TopicCache - auto topic_cache = TopicCache(); - // add topic - topic_cache.addTopic(instance_handler, "TestTopic", "TestType"); - // remove topic - auto did_remove = topic_cache.removeTopic(instance_handler, "TestTopic", "TestType"); + // participant 1 + const auto & it = participant_topic_map.find(this->guid[0]); + ASSERT_TRUE(it != participant_topic_map.end()); + // Verify that the topic and respective types are present + auto & topic_type_map = it->second; + + const auto & topic1it = topic_type_map.find("topic1"); + ASSERT_TRUE(topic1it != topic_type_map.end()); + auto topic_types = topic1it->second; + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); + + const auto & topic2it = topic_type_map.find("topic2"); + ASSERT_TRUE(topic2it != topic_type_map.end()); + topic_types = topic2it->second; + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); + + // participant 2 + const auto & it2 = participant_topic_map.find(this->guid[1]); + ASSERT_TRUE(it2 != participant_topic_map.end()); + // Verify that the topic and respective types are present + auto & topic_type_map2 = it2->second; + + const auto & topic1it2 = topic_type_map2.find("topic1"); + ASSERT_TRUE(topic1it2 != topic_type_map2.end()); + topic_types = topic1it2->second; + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); + + const auto & topic2it2 = topic_type_map2.find("topic2"); + ASSERT_TRUE(topic2it2 != topic_type_map2.end()); + topic_types = topic2it2->second; + EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); +} + +TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) +{ + const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); + auto expected_results = std::map>>(); + expected_results["topic1"].push_back(std::make_tuple(guid[0], "type1", rmw_qos[0])); + expected_results["topic1"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); + expected_results["topic2"].push_back(std::make_tuple(guid[0], "type2", rmw_qos[0])); + expected_results["topic2"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); + for (auto & result_it : expected_results) { + auto & topic_name = result_it.first; + auto & expected_topic_data = result_it.second; + + const auto & it = topic_data_map.find(topic_name); + ASSERT_TRUE(it != topic_data_map.end()); + // Verify that the topic has all the associated data + auto & topic_data = it->second; + for (auto i = 0u; i < expected_topic_data.size(); i++) { + // GUID + EXPECT_EQ(std::get<0>(topic_data.at(i)), std::get<0>(expected_topic_data.at(i))); + // TYPE + EXPECT_EQ(std::get<1>(topic_data.at(i)), std::get<1>(expected_topic_data.at(i))); + // QOS + auto & qos = std::get<2>(topic_data.at(i)); + auto & expected_qos = std::get<2>(expected_topic_data.at(i)); + EXPECT_EQ(qos.durability, expected_qos.durability); + EXPECT_EQ(qos.reliability, expected_qos.reliability); + EXPECT_EQ(qos.liveliness, expected_qos.liveliness); + EXPECT_EQ(qos.liveliness_lease_duration.sec, expected_qos.liveliness_lease_duration.sec); + EXPECT_EQ(qos.liveliness_lease_duration.nsec, expected_qos.liveliness_lease_duration.nsec); + EXPECT_EQ(qos.deadline.sec, expected_qos.deadline.sec); + EXPECT_EQ(qos.deadline.nsec, expected_qos.deadline.nsec); + EXPECT_EQ(qos.lifespan.sec, expected_qos.lifespan.sec); + EXPECT_EQ(qos.lifespan.nsec, expected_qos.lifespan.nsec); + } + } +} + +TEST_F(TopicCacheTestFixture, test_topic_cache_add_topic) +{ + // Add Topic + bool did_add = this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", "TestType", + this->qos[1]); + // Verify that the returned value was true + EXPECT_TRUE(did_add); +} + +TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) +{ + auto did_remove = this->topic_cache.removeTopic(this->instance_handler[0], "topic1", "type1"); // Assert that the return was true ASSERT_TRUE(did_remove); + // Verify it is removed from TopicToTypes + const auto & topic_type_map = this->topic_cache.getTopicToTypes(); + const auto & topic_type_it = topic_type_map.find("topic1"); + ASSERT_TRUE(topic_type_it != topic_type_map.end()); + EXPECT_EQ(std::count(topic_type_it->second.begin(), topic_type_it->second.end(), "type1"), 1); + // Verify it is removed from ParticipantTopicMap + const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); + const auto & participant_topic_it = participant_topic_map.find(this->guid[0]); + ASSERT_TRUE(participant_topic_it != participant_topic_map.end()); + const auto & p_topic_type_it = participant_topic_it->second.find("topic1"); + ASSERT_TRUE(p_topic_type_it == participant_topic_it->second.end()); + // Verify it is removed from TopicNameToTopicTypeMap + const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); + const auto & topic_data_it = topic_data_map.find("topic1"); + ASSERT_TRUE(topic_data_it != topic_data_map.end()); + EXPECT_EQ(topic_data_it->second.size(), 1u); - // Verify that there does not exist a value for the guid in the map - const auto & it = topic_cache.getTopicToTypes().find("TestTopic"); - ASSERT_TRUE(it == topic_cache.getTopicToTypes().end()); + did_remove = this->topic_cache.removeTopic(this->instance_handler[1], "topic1", "type1"); + ASSERT_TRUE(did_remove); + const auto & topic_type_map2 = this->topic_cache.getTopicToTypes(); + const auto & topic_type_it2 = topic_type_map2.find("topic1"); + ASSERT_TRUE(topic_type_it2 == topic_type_map2.end()); + const auto & participant_topic_map2 = this->topic_cache.getParticipantToTopics(); + const auto & participant_topic_it2 = participant_topic_map2.find(this->guid[1]); + ASSERT_TRUE(participant_topic_it2 != participant_topic_map2.end()); + const auto & p_topic_type_it2 = participant_topic_it2->second.find("topic1"); + ASSERT_TRUE(p_topic_type_it2 == participant_topic_it2->second.end()); + const auto & topic_data_map2 = this->topic_cache.getTopicNameToTopicData(); + const auto & topic_data_it2 = topic_data_map2.find("topic1"); + ASSERT_TRUE(topic_data_it2 == topic_data_map2.end()); } -TEST(TopicCacheTest, test_topic_cache_remove_policy_element_does_not_exist) { - // Create an instance handler - const auto guid = GUID_t(GuidPrefix_t(), 3); - InstanceHandle_t instance_handler = InstanceHandle_t(); - instance_handler = guid; - - // Create an instance of TopicCache - auto topic_cache = TopicCache(); +TEST_F(TopicCacheTestFixture, test_topic_cache_remove_policy_element_does_not_exist) +{ // add topic - topic_cache.addTopic(instance_handler, "TestTopic", "TestType"); + this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", "TestType", this->qos[1]); // Assert that the return was false - auto const did_remove = topic_cache.removeTopic(instance_handler, "NewTestTopic", "TestType"); + auto const did_remove = this->topic_cache.removeTopic(this->instance_handler[1], "NewTestTopic", + "TestType"); ASSERT_FALSE(did_remove); } From d0ac7e0adfb03af3b9531eca68b0bca83ebc97ee Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Wed, 16 Oct 2019 15:01:59 -0700 Subject: [PATCH 04/23] Stubs for rmw_get_publishers_info_by_topic and rmw_get_subscriptions_info_by_topic Signed-off-by: Jaison Titus --- rmw_fastrtps_cpp/CMakeLists.txt | 1 + rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp | 45 +++++++++++++++++++ rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 1 + .../src/rmw_get_topic_info.cpp | 45 +++++++++++++++++++ rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 20 +++++++++ .../src/rmw_get_topic_info.cpp | 42 +++++++++++++++++ 7 files changed, 155 insertions(+) create mode 100644 rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp create mode 100644 rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 108e67071..05e113f4f 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -81,6 +81,7 @@ add_library(rmw_fastrtps_cpp src/rmw_wait_set.cpp src/serialization_format.cpp src/type_support_common.cpp + src/rmw_get_topic_info.cpp ) target_link_libraries(rmw_fastrtps_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp new file mode 100644 index 000000000..daa2e1383 --- /dev/null +++ b/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp @@ -0,0 +1,45 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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/rmw.h" +#include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_publishers_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * publishers_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_publishers_info_by_topic( + eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, publishers_info); +} + +rmw_ret_t +rmw_get_subscriptions_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * subscriptions_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic( + eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index f2ac936a8..79b02d149 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -85,6 +85,7 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_wait_set.cpp src/type_support_common.cpp src/serialization_format.cpp + src/rmw_get_topic_info.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp new file mode 100644 index 000000000..96b626e3e --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp @@ -0,0 +1,45 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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/rmw.h" +#include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_publishers_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * publishers_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_publishers_info_by_topic( + eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, publishers_info); +} + +rmw_ret_t +rmw_get_subscriptions_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * subscriptions_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic( + eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info); +} +} // extern "C" diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index efa321401..f41917078 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -75,6 +75,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_wait.cpp src/rmw_wait_set.cpp src/TypeSupport_impl.cpp + src/rmw_get_topic_info.cpp ) target_link_libraries(rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 7c01a6d08..d54d2ce41 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -349,6 +349,26 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_publishers_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * publishers_info); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_subscriptions_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * subscriptions_info); + } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp new file mode 100644 index 000000000..851b3dc62 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -0,0 +1,42 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_publishers_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * publishers_info) +{ + return RMW_RET_UNSUPPORTED; +} + +rmw_ret_t +__rmw_get_subscriptions_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_info_array_t * subscriptions_info) +{ + return RMW_RET_UNSUPPORTED; +} +} // namespace rmw_fastrtps_shared_cpp From e6404795946b417afd6b67e66c86abc1bf5eef64 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 22 Oct 2019 18:32:02 -0700 Subject: [PATCH 05/23] Implemented rmw_get_publishers_info_by_topic and rmw_get_subscriptions_info_by_topic Signed-off-by: Jaison Titus --- .../custom_participant_info.hpp | 7 +- .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 3 +- .../src/rmw_get_topic_info.cpp | 197 +++++++++++++++++- .../test/test_topic_cache.cpp | 28 ++- 4 files changed, 218 insertions(+), 17 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index b66510e37..478c89a57 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -164,7 +164,6 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener { auto & topic_cache = is_reader ? reader_topic_cache : writer_topic_cache; - bool trigger; { std::lock_guard guard(topic_cache.getMutex()); @@ -175,8 +174,10 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener proxyData.typeName().to_string(), proxyData.m_qos); } else { - trigger = topic_cache().removeTopic(proxyData.RTPSParticipantKey(), - proxyData.topicName().to_string(), proxyData.typeName().to_string()); + trigger = topic_cache().removeTopic( + proxyData.RTPSParticipantKey(), + proxyData.topicName().to_string(), + proxyData.typeName().to_string()); } } if (trigger) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index ff37eb6a7..a71a52958 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -50,7 +50,7 @@ class TopicCache * Map of topic names to TopicData. Where topic data is vector of tuples containing * participant GUID, topic type and the qos policy of the respective participant. * Topics here are represented as one to many, DDS XTypes 1.2 - * specifies application code 'generally' uses a 1-1 relationship. + * specifies application code that 'generally' uses a 1-1 relationship. * However, generic services such as logger and monitor, can discover * multiple types on the same topic. */ @@ -142,6 +142,7 @@ class TopicCache * @param rtpsParticipantKey * @param topic_name * @param type_name + * @param dds_qos the dds qos policy of the participant * @return true if a change has been recorded */ template diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 851b3dc62..7abeb58cd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -12,10 +12,189 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include +#include +#include + +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/topic_info_array.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" namespace rmw_fastrtps_shared_cpp { + +rmw_ret_t +_validate_params( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + rmw_topic_info_array_t * participants_info) +{ + if (!identifier) { + RMW_SET_ERROR_MSG("null implementation identifier provided"); + return RMW_RET_ERROR; + } + + if (!topic_name) { + RMW_SET_ERROR_MSG("null topic_name provided"); + return RMW_RET_ERROR; + } + + if (!allocator) { + RMW_SET_ERROR_MSG("null allocator provided"); + return RMW_RET_ERROR; + } + + if (!node) { + RMW_SET_ERROR_MSG("null node handle"); + return RMW_RET_ERROR; + } + + // Get participant pointer from node + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + if (!participants_info) { + RMW_SET_ERROR_MSG("null participants_info provided"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +std::vector +_get_topic_fqdns(const char * topic_name, bool no_mangle) +{ + std::vector topic_fqdns; + topic_fqdns.push_back(topic_name); + // if mangle + if (!no_mangle) { + auto ros_prefixes = _get_all_ros_prefixes(); + // Build the list of all possible topic FQDN + std::for_each(ros_prefixes.begin(), ros_prefixes.end(), + [&topic_fqdns, &topic_name](const std::string & prefix) { + topic_fqdns.push_back(prefix + topic_name); + }); + } + return topic_fqdns; +} + +rmw_ret_t +_set_rmw_topic_info( + rcutils_allocator_t * allocator, + std::tuple data, + ::ParticipantListener * slave_target, + rmw_topic_info_t * topic_info) +{ + const auto & gid = std::get<0>(data); + // convert gid to const char * and set it inside topic_info + std::ostringstream gid_stream; + gid_stream << gid; + rmw_ret_t ret = rmw_topic_info_set_gid(allocator, gid_stream.str().c_str(), topic_info); + if (ret != RMW_RET_OK) { + return ret; + } + // set topic type + ret = rmw_topic_info_set_topic_type(allocator, std::get<1>(data).c_str(), topic_info); + if (ret != RMW_RET_OK) { + return ret; + } + // set qos profile + ret = rmw_topic_info_set_qos_profile(&std::get<2>(data), topic_info); + if (ret != RMW_RET_OK) { + return ret; + } + // set node name + const auto & d_name_it = slave_target->discovered_names.find(gid); + if (d_name_it != slave_target->discovered_names.end()) { + ret = rmw_topic_info_set_node_name(allocator, d_name_it->second.c_str(), topic_info); + } else { + ret = rmw_topic_info_set_node_name(allocator, "_NODE_NAME_UNKNOWN_", topic_info); + } + if (ret != RMW_RET_OK) { + return ret; + } + // set node namespace + const auto & d_namespace_it = slave_target->discovered_namespaces.find(gid); + if (d_namespace_it != slave_target->discovered_namespaces.end()) { + ret = rmw_topic_info_set_node_namespace(allocator, d_namespace_it->second.c_str(), topic_info); + } else { + ret = rmw_topic_info_set_node_namespace(allocator, "_NODE_NAMESPACE_UNKNOWN_", topic_info); + } + return ret; +} + + +rmw_ret_t +_get_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + bool is_publisher, + rmw_topic_info_array_t * participants_info) +{ + rmw_ret_t ret = _validate_params( + identifier, + node, + allocator, + topic_name, + participants_info); + if (ret != RMW_RET_OK) { + return ret; + } + + const auto & topic_fqdns = _get_topic_fqdns(topic_name, no_mangle); + + auto impl = static_cast(node->data); + ::ParticipantListener * slave_target = impl->listener; + auto & topic_cache = + is_publisher ? slave_target->writer_topic_cache : slave_target->reader_topic_cache; + { + std::lock_guard guard(topic_cache.getMutex()); + auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); + std::vector topic_info_vector; + for (const auto & topic_name : topic_fqdns) { + const auto & it = topic_name_to_data.find(topic_name); + if (it != topic_name_to_data.end()) { + for (const auto & data : it->second) { + rmw_topic_info_t topic_info; + rmw_ret_t ret = _set_rmw_topic_info(allocator, data, slave_target, &topic_info); + if (ret != RMW_RET_OK) { + RMW_SET_ERROR_MSG("Failed to create set_rmw_topic_info."); + return ret; + } + // add rmw_topic_info_t to a vector + topic_info_vector.push_back(topic_info); + } + } + } + + // add all the elements from the vector to rmw_topic_info_array_t + auto count = topic_info_vector.size(); + ret = rmw_topic_info_array_init_with_size(allocator, count, participants_info); + if (ret != RMW_RET_OK) { + RMW_SET_ERROR_MSG("rmw_topic_info_array_init_with_size failed to allocate memory."); + return RMW_RET_BAD_ALLOC; + } + for (auto i = 0u; i < count; i++) { + participants_info->info_array[i] = topic_info_vector.at(i); + } + participants_info->count = count; + } + return RMW_RET_OK; +} + rmw_ret_t __rmw_get_publishers_info_by_topic( const char * identifier, @@ -25,7 +204,14 @@ __rmw_get_publishers_info_by_topic( bool no_mangle, rmw_topic_info_array_t * publishers_info) { - return RMW_RET_UNSUPPORTED; + return _get_info_by_topic( + identifier, + node, + allocator, + topic_name, + no_mangle, + true, /*is_publisher*/ + publishers_info); } rmw_ret_t @@ -37,6 +223,13 @@ __rmw_get_subscriptions_info_by_topic( bool no_mangle, rmw_topic_info_array_t * subscriptions_info) { - return RMW_RET_UNSUPPORTED; + return _get_info_by_topic( + identifier, + node, + allocator, + topic_name, + no_mangle, + false, /*is_publisher*/ + subscriptions_info); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index c49bfcda8..74c54237c 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -41,12 +41,17 @@ class TopicCacheTestFixture : public ::testing::Test GUID_t guid[2]; void SetUp() { - // Create an instance handlers + // Create instance handlers for (int i = 0; i < 2; i++) { guid[i] = GUID_t(GuidPrefix_t(), i + 1); instance_handler[i] = guid[i]; } + // Populating WriterQos -> which is from the DDS layer and + // rmw_qos_profile_t which is from rmw/types.h. + // This is done to test if topic_cache.getTopicNameToTopicData() returns + // the correct value in rmw_qos_profile_t for a given WriterQos + // DDS qos qos[0].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; qos[0].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; @@ -85,7 +90,7 @@ class TopicCacheTestFixture : public ::testing::Test rmw_qos[1].lifespan.sec = 19u; rmw_qos[1].lifespan.nsec = 5432u; - // Add some topics + // Add data to topic_cache topic_cache.addTopic(instance_handler[0], "topic1", "type1", qos[0]); topic_cache.addTopic(instance_handler[0], "topic2", "type2", qos[0]); topic_cache.addTopic(instance_handler[1], "topic1", "type1", qos[1]); @@ -124,7 +129,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) const auto & it = participant_topic_map.find(this->guid[0]); ASSERT_TRUE(it != participant_topic_map.end()); // Verify that the topic and respective types are present - auto & topic_type_map = it->second; + const auto & topic_type_map = it->second; const auto & topic1it = topic_type_map.find("topic1"); ASSERT_TRUE(topic1it != topic_type_map.end()); @@ -140,7 +145,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) const auto & it2 = participant_topic_map.find(this->guid[1]); ASSERT_TRUE(it2 != participant_topic_map.end()); // Verify that the topic and respective types are present - auto & topic_type_map2 = it2->second; + const auto & topic_type_map2 = it2->second; const auto & topic1it2 = topic_type_map2.find("topic1"); ASSERT_TRUE(topic1it2 != topic_type_map2.end()); @@ -163,21 +168,21 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) expected_results["topic2"].push_back(std::make_tuple(guid[0], "type2", rmw_qos[0])); expected_results["topic2"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); for (auto & result_it : expected_results) { - auto & topic_name = result_it.first; - auto & expected_topic_data = result_it.second; + const auto & topic_name = result_it.first; + const auto & expected_topic_data = result_it.second; const auto & it = topic_data_map.find(topic_name); ASSERT_TRUE(it != topic_data_map.end()); // Verify that the topic has all the associated data - auto & topic_data = it->second; + const auto & topic_data = it->second; for (auto i = 0u; i < expected_topic_data.size(); i++) { // GUID EXPECT_EQ(std::get<0>(topic_data.at(i)), std::get<0>(expected_topic_data.at(i))); // TYPE EXPECT_EQ(std::get<1>(topic_data.at(i)), std::get<1>(expected_topic_data.at(i))); // QOS - auto & qos = std::get<2>(topic_data.at(i)); - auto & expected_qos = std::get<2>(expected_topic_data.at(i)); + const auto & qos = std::get<2>(topic_data.at(i)); + const auto & expected_qos = std::get<2>(expected_topic_data.at(i)); EXPECT_EQ(qos.durability, expected_qos.durability); EXPECT_EQ(qos.reliability, expected_qos.reliability); EXPECT_EQ(qos.liveliness, expected_qos.liveliness); @@ -194,7 +199,8 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) TEST_F(TopicCacheTestFixture, test_topic_cache_add_topic) { // Add Topic - bool did_add = this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", "TestType", + const bool did_add = this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", + "TestType", this->qos[1]); // Verify that the returned value was true EXPECT_TRUE(did_add); @@ -242,7 +248,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_remove_policy_element_does_not_ex // add topic this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", "TestType", this->qos[1]); // Assert that the return was false - auto const did_remove = this->topic_cache.removeTopic(this->instance_handler[1], "NewTestTopic", + const auto did_remove = this->topic_cache.removeTopic(this->instance_handler[1], "NewTestTopic", "TestType"); ASSERT_FALSE(did_remove); } From 1a80e446dfb7209e2a6cc49998e2e14542d49916 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 14 Nov 2019 13:50:14 -0800 Subject: [PATCH 06/23] - Addressing code review changes: Using const auto instead of auto Signed-off-by: Jaison Titus --- rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index 74c54237c..8b907709b 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -167,7 +167,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) expected_results["topic1"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); expected_results["topic2"].push_back(std::make_tuple(guid[0], "type2", rmw_qos[0])); expected_results["topic2"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); - for (auto & result_it : expected_results) { + for (const auto & result_it : expected_results) { const auto & topic_name = result_it.first; const auto & expected_topic_data = result_it.second; From 30a2a562ee1326ed5b9e6ce5217cf2a17da6ba87 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 15 Nov 2019 16:55:39 -0800 Subject: [PATCH 07/23] - Alpha sorted cmakelist.txt - changed to using instead of typedef Signed-off-by: Jaison Titus --- rmw_fastrtps_shared_cpp/CMakeLists.txt | 2 +- .../include/rmw_fastrtps_shared_cpp/topic_cache.hpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index f41917078..ac239dd0e 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -56,6 +56,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_compare_gids_equal.cpp src/rmw_count.cpp src/rmw_get_gid_for_publisher.cpp + src/rmw_get_topic_info.cpp src/rmw_guard_condition.cpp src/rmw_logging.cpp src/rmw_node.cpp @@ -75,7 +76,6 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_wait.cpp src/rmw_wait_set.cpp src/TypeSupport_impl.cpp - src/rmw_get_topic_info.cpp ) target_link_libraries(rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index a71a52958..0a3f82f22 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -41,10 +41,10 @@ typedef eprosima::fastrtps::rtps::GUID_t GUID_t; class TopicCache { private: - typedef std::unordered_map> TopicToTypes; - typedef std::map ParticipantTopicMap; - typedef std::vector> TopicData; - typedef std::unordered_map TopicNameToTopicData; + using TopicToTypes = std::unordered_map>; + using ParticipantTopicMap = std::map; + using TopicData = std::vector>; + using TopicNameToTopicData = std::unordered_map; /** * Map of topic names to TopicData. Where topic data is vector of tuples containing From dbfe0b4ce9e71a39c0135cbac83c257921f53f77 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 15 Nov 2019 17:19:26 -0800 Subject: [PATCH 08/23] - Alphasort Signed-off-by: Jaison Titus --- rmw_fastrtps_cpp/CMakeLists.txt | 2 +- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 05e113f4f..55211e715 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -60,6 +60,7 @@ add_library(rmw_fastrtps_cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_implementation_identifier.cpp src/rmw_get_serialization_format.cpp + src/rmw_get_topic_info.cpp src/rmw_guard_condition.cpp src/rmw_init.cpp src/rmw_node.cpp @@ -81,7 +82,6 @@ add_library(rmw_fastrtps_cpp src/rmw_wait_set.cpp src/serialization_format.cpp src/type_support_common.cpp - src/rmw_get_topic_info.cpp ) target_link_libraries(rmw_fastrtps_cpp fastcdr fastrtps) diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 79b02d149..024d834d1 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -64,6 +64,7 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_implementation_identifier.cpp src/rmw_get_serialization_format.cpp + src/rmw_get_topic_info.cpp src/rmw_guard_condition.cpp src/rmw_init.cpp src/rmw_node.cpp @@ -85,7 +86,6 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_wait_set.cpp src/type_support_common.cpp src/serialization_format.cpp - src/rmw_get_topic_info.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) From 04b29c872fc6d6a1864c0125d10decd4d96f1719 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 22 Nov 2019 10:51:53 -0800 Subject: [PATCH 09/23] - Refactor to accomodate change in rmw_topic_info_set* functions. - Refactor to accomodate change in rmw_topic_info_array* functions. Signed-off-by: Jaison Titus --- .../src/rmw_get_topic_info.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 7abeb58cd..f852479e6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -21,6 +21,7 @@ #include "rmw/rmw.h" #include "rmw/types.h" #include "rmw/topic_info_array.h" +#include "rmw/topic_info.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -76,7 +77,6 @@ _get_topic_fqdns(const char * topic_name, bool no_mangle) { std::vector topic_fqdns; topic_fqdns.push_back(topic_name); - // if mangle if (!no_mangle) { auto ros_prefixes = _get_all_ros_prefixes(); // Build the list of all possible topic FQDN @@ -99,12 +99,12 @@ _set_rmw_topic_info( // convert gid to const char * and set it inside topic_info std::ostringstream gid_stream; gid_stream << gid; - rmw_ret_t ret = rmw_topic_info_set_gid(allocator, gid_stream.str().c_str(), topic_info); + rmw_ret_t ret = rmw_topic_info_set_gid(topic_info, gid_stream.str().c_str(), allocator); if (ret != RMW_RET_OK) { return ret; } // set topic type - ret = rmw_topic_info_set_topic_type(allocator, std::get<1>(data).c_str(), topic_info); + ret = rmw_topic_info_set_topic_type(topic_info, std::get<1>(data).c_str(), allocator); if (ret != RMW_RET_OK) { return ret; } @@ -116,9 +116,9 @@ _set_rmw_topic_info( // set node name const auto & d_name_it = slave_target->discovered_names.find(gid); if (d_name_it != slave_target->discovered_names.end()) { - ret = rmw_topic_info_set_node_name(allocator, d_name_it->second.c_str(), topic_info); + ret = rmw_topic_info_set_node_name(topic_info, d_name_it->second.c_str(), allocator); } else { - ret = rmw_topic_info_set_node_name(allocator, "_NODE_NAME_UNKNOWN_", topic_info); + ret = rmw_topic_info_set_node_name(topic_info, "_NODE_NAME_UNKNOWN_", allocator); } if (ret != RMW_RET_OK) { return ret; @@ -126,9 +126,9 @@ _set_rmw_topic_info( // set node namespace const auto & d_namespace_it = slave_target->discovered_namespaces.find(gid); if (d_namespace_it != slave_target->discovered_namespaces.end()) { - ret = rmw_topic_info_set_node_namespace(allocator, d_namespace_it->second.c_str(), topic_info); + ret = rmw_topic_info_set_node_namespace(topic_info, d_namespace_it->second.c_str(), allocator); } else { - ret = rmw_topic_info_set_node_namespace(allocator, "_NODE_NAMESPACE_UNKNOWN_", topic_info); + ret = rmw_topic_info_set_node_namespace(topic_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); } return ret; } @@ -182,7 +182,7 @@ _get_info_by_topic( // add all the elements from the vector to rmw_topic_info_array_t auto count = topic_info_vector.size(); - ret = rmw_topic_info_array_init_with_size(allocator, count, participants_info); + ret = rmw_topic_info_array_init_with_size(participants_info, count, allocator); if (ret != RMW_RET_OK) { RMW_SET_ERROR_MSG("rmw_topic_info_array_init_with_size failed to allocate memory."); return RMW_RET_BAD_ALLOC; From af1e536f8f08ed257cb0d94e709d50e663eca893 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 22 Nov 2019 14:53:33 -0800 Subject: [PATCH 10/23] Refactor to accomodate change of gid from const char * to uint8_t[] Signed-off-by: Jaison Titus --- .../src/rmw_get_topic_info.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index f852479e6..60c96d35d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -96,10 +96,15 @@ _set_rmw_topic_info( rmw_topic_info_t * topic_info) { const auto & gid = std::get<0>(data); - // convert gid to const char * and set it inside topic_info - std::ostringstream gid_stream; - gid_stream << gid; - rmw_ret_t ret = rmw_topic_info_set_gid(topic_info, gid_stream.str().c_str(), allocator); + static_assert( + sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." + ); + uint8_t rmw_gid[RMW_GID_STORAGE_SIZE]; + memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); + memcpy(&rmw_gid, &gid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + rmw_ret_t ret = rmw_topic_info_set_gid(topic_info, rmw_gid, + sizeof(eprosima::fastrtps::rtps::GUID_t)); if (ret != RMW_RET_OK) { return ret; } @@ -109,7 +114,7 @@ _set_rmw_topic_info( return ret; } // set qos profile - ret = rmw_topic_info_set_qos_profile(&std::get<2>(data), topic_info); + ret = rmw_topic_info_set_qos_profile(topic_info, &std::get<2>(data)); if (ret != RMW_RET_OK) { return ret; } From e87e83c582b06789b9e78a386b9596b981ee2cc0 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 22 Nov 2019 14:53:59 -0800 Subject: [PATCH 11/23] Moved dds_qos_to_rmw_qos from qos_converter to qos.hpp Signed-off-by: Jaison Titus --- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 64 ++++++++++++++ .../rmw_fastrtps_shared_cpp/qos_converter.hpp | 86 ------------------- .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 2 +- rmw_fastrtps_shared_cpp/src/qos.cpp | 1 - 4 files changed, 65 insertions(+), 88 deletions(-) delete mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index 1139413cb..fed276204 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -18,6 +18,8 @@ #include "rmw/rmw.h" #include "./visibility_control.h" +#include "fastrtps/qos/WriterQos.h" +#include "fastrtps/qos/ReaderQos.h" namespace eprosima { @@ -44,6 +46,68 @@ get_datawriter_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr); +/* + * Converts the low-level QOS Policy; of type WriterQos or ReaderQos into rmw_qos_profile_t. + * Since WriterQos or ReaderQos does not have information about history and depth, these values are not set + * by this function. + * + * \param[in] dds_qos of type WriterQos or ReaderQos + * \param[out] qos the equivalent of the data in WriterQos or ReaderQos in rmw_qos_profile_t + */ +template +void +dds_qos_to_rmw_qos( + const DDSQoSPolicyT & dds_qos, + rmw_qos_profile_t * qos) +{ + switch (dds_qos.m_reliability.kind) { + case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + default: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + break; + } + + switch (dds_qos.m_durability.kind) { + case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + default: + qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + + qos->deadline.sec = dds_qos.m_deadline.period.seconds; + qos->deadline.nsec = dds_qos.m_deadline.period.nanosec; + + qos->lifespan.sec = dds_qos.m_lifespan.duration.seconds; + qos->lifespan.nsec = dds_qos.m_lifespan.duration.nanosec; + + switch (dds_qos.m_liveliness.kind) { + case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case eprosima::fastrtps::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + break; + case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + break; + default: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + break; + } + qos->liveliness_lease_duration.sec = dds_qos.m_liveliness.lease_duration.seconds; + qos->liveliness_lease_duration.nsec = dds_qos.m_liveliness.lease_duration.nanosec; +} + template void dds_attributes_to_rmw_qos( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp deleted file mode 100644 index c78ddb6c9..000000000 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos_converter.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// 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_FASTRTPS_SHARED_CPP__QOS_CONVERTER_HPP_ -#define RMW_FASTRTPS_SHARED_CPP__QOS_CONVERTER_HPP_ - -#include "rmw/types.h" - -#include "fastrtps/qos/WriterQos.h" -#include "fastrtps/qos/ReaderQos.h" - -/* - * Converts the low-level QOS Policy; of type WriterQos or ReaderQos into rmw_qos_profile_t. - * Since WriterQos or ReaderQos does not have information about history and depth, these values are not set - * by this function. - * - * \param[in] dds_qos of type WriterQos or ReaderQos - * \param[out] qos the equivalent of the data in WriterQos or ReaderQos in rmw_qos_profile_t - */ -template -void -dds_qos_to_rmw_qos( - const DDSQoSPolicyT & dds_qos, - rmw_qos_profile_t * qos) -{ - switch (dds_qos.m_reliability.kind) { - case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: - qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - break; - case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: - qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - break; - default: - qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - break; - } - - switch (dds_qos.m_durability.kind) { - case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: - qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - break; - case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: - qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; - break; - default: - qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - break; - } - - qos->deadline.sec = dds_qos.m_deadline.period.seconds; - qos->deadline.nsec = dds_qos.m_deadline.period.nanosec; - - qos->lifespan.sec = dds_qos.m_lifespan.duration.seconds; - qos->lifespan.nsec = dds_qos.m_lifespan.duration.nanosec; - - switch (dds_qos.m_liveliness.kind) { - case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - break; - case eprosima::fastrtps::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; - break; - case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - break; - default: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - break; - } - qos->liveliness_lease_duration.sec = dds_qos.m_liveliness.lease_duration.seconds; - qos->liveliness_lease_duration.nsec = dds_qos.m_liveliness.lease_duration.nanosec; -} - -#endif // RMW_FASTRTPS_SHARED_CPP__QOS_CONVERTER_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index 0a3f82f22..b4cbc6713 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -29,7 +29,7 @@ #include "fastrtps/participant/Participant.h" #include "fastrtps/rtps/common/Guid.h" #include "fastrtps/rtps/common/InstanceHandle.h" -#include "qos_converter.hpp" +#include "qos.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 72aaece31..e6ca5a20c 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -14,7 +14,6 @@ #include -#include "rmw_fastrtps_shared_cpp/qos_converter.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "fastrtps/attributes/PublisherAttributes.h" From 16fcaede2f5899058ea53d441de1fc370654fa1b Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 22 Nov 2019 16:06:33 -0800 Subject: [PATCH 12/23] - Modified topic cache to store participant guid and publisher/subscription guid seperately - Modified the tests to reflect this change - Fixed bug where get_topic_info was not accounting for calls from the same participant resulting in node name and namespaces not being found. Signed-off-by: Jaison Titus --- .../custom_participant_info.hpp | 2 + .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 56 ++++++++++------ .../src/rmw_get_topic_info.cpp | 47 ++++++++++--- .../test/test_topic_cache.cpp | 67 +++++++++++-------- 4 files changed, 113 insertions(+), 59 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 478c89a57..3ce19dbd3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -170,12 +170,14 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener if (is_alive) { trigger = topic_cache().addTopic( proxyData.RTPSParticipantKey(), + proxyData.guid(), proxyData.topicName().to_string(), proxyData.typeName().to_string(), proxyData.m_qos); } else { trigger = topic_cache().removeTopic( proxyData.RTPSParticipantKey(), + proxyData.guid(), proxyData.topicName().to_string(), proxyData.typeName().to_string()); } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index b4cbc6713..f2176a016 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -35,6 +35,18 @@ typedef eprosima::fastrtps::rtps::GUID_t GUID_t; +/** + * A data structure that encapsulates all the data associated with a publisher + * or subscription by the topic it publishers or subscribers to + */ +struct TopicData +{ + GUID_t participant_guid; + GUID_t guid; /* publisher or subscription guid */ + std::string topic_type; + rmw_qos_profile_t qos_profile; +}; + /** * Topic cache data structure. Manages relationships between participants and topics. */ @@ -43,8 +55,7 @@ class TopicCache private: using TopicToTypes = std::unordered_map>; using ParticipantTopicMap = std::map; - using TopicData = std::vector>; - using TopicNameToTopicData = std::unordered_map; + using TopicNameToTopicData = std::unordered_map>; /** * Map of topic names to TopicData. Where topic data is vector of tuples containing @@ -72,7 +83,7 @@ class TopicCache TopicNameToTopicData & topic_name_to_topic_data) { if (topic_name_to_topic_data.find(topic_name) == topic_name_to_topic_data.end()) { - topic_name_to_topic_data[topic_name] = TopicData(); + topic_name_to_topic_data[topic_name] = std::vector(); } } @@ -113,8 +124,8 @@ class TopicCache TopicToTypes topic_to_types; for (const auto & it : topic_name_to_topic_data_) { topic_to_types[it.first] = std::vector(); - for (const auto & mit : it.second) { - topic_to_types[it.first].push_back(std::get<1>(mit)); + for (const auto & topic_data : it.second) { + topic_to_types[it.first].push_back(topic_data.topic_type); } } return topic_to_types; @@ -148,13 +159,14 @@ class TopicCache template bool addTopic( const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, + const GUID_t & guid, /* publisher or subscription GUID */ const std::string & topic_name, const std::string & type_name, const T & dds_qos) { initializeTopicDataMap(topic_name, topic_name_to_topic_data_); - auto guid = iHandle2GUID(rtpsParticipantKey); - initializeParticipantMap(participant_to_topics_, guid); + auto participant_guid = iHandle2GUID(rtpsParticipantKey); + initializeParticipantMap(participant_to_topics_, participant_guid); initializeTopicTypesMap(topic_name, participant_to_topics_[guid]); if (rcutils_logging_logger_is_enabled_for("rmw_fastrtps_shared_cpp", RCUTILS_LOG_SEVERITY_DEBUG)) @@ -166,10 +178,16 @@ class TopicCache "Adding topic '%s' with type '%s' for node '%s'", topic_name.c_str(), type_name.c_str(), guid_stream.str().c_str()); } - auto qos_profile = rmw_qos_profile_t(); + rmw_qos_profile_t qos_profile = rmw_qos_profile_t(); dds_qos_to_rmw_qos(dds_qos, &qos_profile); - topic_name_to_topic_data_[topic_name].push_back(std::make_tuple(guid, type_name, qos_profile)); - participant_to_topics_[guid][topic_name].push_back(type_name); + TopicData topic_data = { + participant_guid, + guid, + type_name, + qos_profile + }; + topic_name_to_topic_data_[topic_name].push_back(topic_data); + participant_to_topics_[participant_guid][topic_name].push_back(type_name); return true; } @@ -177,12 +195,14 @@ class TopicCache * Remove a topic based on discovery. * * @param rtpsParticipantKey + * @param guid the guid of the publisher or subscription * @param topic_name * @param type_name * @return true if a change has been recorded */ bool removeTopic( const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, + const eprosima::fastrtps::rtps::GUID_t & guid, const std::string & topic_name, const std::string & type_name) { @@ -193,30 +213,28 @@ class TopicCache topic_name.c_str(), type_name.c_str()); return false; } - auto guid = iHandle2GUID(rtpsParticipantKey); { auto & type_vec = topic_name_to_topic_data_[topic_name]; type_vec.erase(std::find_if(type_vec.begin(), type_vec.end(), - [type_name, guid](const auto & topic_info) { - return type_name.compare(std::get<1>(topic_info)) == 0 && - guid == std::get<0>(topic_info); + [type_name, guid](const auto & topic_data) { + return type_name.compare(topic_data.topic_type) == 0 && guid == topic_data.guid; })); if (type_vec.empty()) { topic_name_to_topic_data_.erase(topic_name); } } - - auto guid_topics_pair = participant_to_topics_.find(guid); + auto participant_guid = iHandle2GUID(rtpsParticipantKey); + auto guid_topics_pair = participant_to_topics_.find(participant_guid); if (guid_topics_pair != participant_to_topics_.end() && guid_topics_pair->second.find(topic_name) != guid_topics_pair->second.end()) { auto & type_vec = guid_topics_pair->second[topic_name]; type_vec.erase(std::find(type_vec.begin(), type_vec.end(), type_name)); if (type_vec.empty()) { - participant_to_topics_[guid].erase(topic_name); + participant_to_topics_[participant_guid].erase(topic_name); } - if (participant_to_topics_[guid].empty()) { - participant_to_topics_.erase(guid); + if (participant_to_topics_[participant_guid].empty()) { + participant_to_topics_.erase(participant_guid); } } else { RCUTILS_LOG_DEBUG_NAMED( diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 60c96d35d..9b37ce9bf 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -90,36 +90,50 @@ _get_topic_fqdns(const char * topic_name, bool no_mangle) rmw_ret_t _set_rmw_topic_info( - rcutils_allocator_t * allocator, - std::tuple data, + rmw_topic_info_t * topic_info, + const GUID_t & participant_guid, + const char * node_name, + const char * node_namespace, + TopicData topic_data, ::ParticipantListener * slave_target, - rmw_topic_info_t * topic_info) + rcutils_allocator_t * allocator) { - const auto & gid = std::get<0>(data); static_assert( sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." ); uint8_t rmw_gid[RMW_GID_STORAGE_SIZE]; memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(&rmw_gid, &gid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + memcpy(&rmw_gid, &topic_data.guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); rmw_ret_t ret = rmw_topic_info_set_gid(topic_info, rmw_gid, sizeof(eprosima::fastrtps::rtps::GUID_t)); if (ret != RMW_RET_OK) { return ret; } // set topic type - ret = rmw_topic_info_set_topic_type(topic_info, std::get<1>(data).c_str(), allocator); + ret = rmw_topic_info_set_topic_type(topic_info, topic_data.topic_type.c_str(), allocator); if (ret != RMW_RET_OK) { return ret; } // set qos profile - ret = rmw_topic_info_set_qos_profile(topic_info, &std::get<2>(data)); + ret = rmw_topic_info_set_qos_profile(topic_info, &topic_data.qos_profile); if (ret != RMW_RET_OK) { return ret; } + // Check if this participant is the same as the node that is passed + if (topic_data.participant_guid == participant_guid) { + ret = rmw_topic_info_set_node_name(topic_info, node_name, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = rmw_topic_info_set_node_namespace(topic_info, node_namespace, allocator); + return ret; + } + // This means that this discovered participant is not associated with the passed node + // and hence we must find its name and namespace from the discovered_names and + // discovered_namespace maps // set node name - const auto & d_name_it = slave_target->discovered_names.find(gid); + const auto & d_name_it = slave_target->discovered_names.find(topic_data.participant_guid); if (d_name_it != slave_target->discovered_names.end()) { ret = rmw_topic_info_set_node_name(topic_info, d_name_it->second.c_str(), allocator); } else { @@ -129,7 +143,8 @@ _set_rmw_topic_info( return ret; } // set node namespace - const auto & d_namespace_it = slave_target->discovered_namespaces.find(gid); + const auto & d_namespace_it = + slave_target->discovered_namespaces.find(topic_data.participant_guid); if (d_namespace_it != slave_target->discovered_namespaces.end()) { ret = rmw_topic_info_set_node_namespace(topic_info, d_namespace_it->second.c_str(), allocator); } else { @@ -160,8 +175,11 @@ _get_info_by_topic( } const auto & topic_fqdns = _get_topic_fqdns(topic_name, no_mangle); - auto impl = static_cast(node->data); + // The GUID of the participant associated with this node + const auto & participant_guid = impl->participant->getGuid(); + const auto & node_name = node->name; + const auto & node_namespace = node->namespace_; ::ParticipantListener * slave_target = impl->listener; auto & topic_cache = is_publisher ? slave_target->writer_topic_cache : slave_target->reader_topic_cache; @@ -174,7 +192,14 @@ _get_info_by_topic( if (it != topic_name_to_data.end()) { for (const auto & data : it->second) { rmw_topic_info_t topic_info; - rmw_ret_t ret = _set_rmw_topic_info(allocator, data, slave_target, &topic_info); + rmw_ret_t ret = _set_rmw_topic_info( + &topic_info, + participant_guid, + node_name, + node_namespace, + data, + slave_target, + allocator); if (ret != RMW_RET_OK) { RMW_SET_ERROR_MSG("Failed to create set_rmw_topic_info."); return ret; diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index 8b907709b..094d9ab75 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -37,14 +37,16 @@ class TopicCacheTestFixture : public ::testing::Test TopicCache topic_cache; WriterQos qos[2]; rmw_qos_profile_t rmw_qos[2]; - InstanceHandle_t instance_handler[2]; + InstanceHandle_t participant_instance_handler[2]; + GUID_t participant_guid[2]; GUID_t guid[2]; void SetUp() { // Create instance handlers for (int i = 0; i < 2; i++) { - guid[i] = GUID_t(GuidPrefix_t(), i + 1); - instance_handler[i] = guid[i]; + guid[i] = GUID_t(GuidPrefix_t(), i + 100); + participant_guid[i] = GUID_t(GuidPrefix_t(), i + 1); + participant_instance_handler[i] = participant_guid[i]; } // Populating WriterQos -> which is from the DDS layer and @@ -91,10 +93,10 @@ class TopicCacheTestFixture : public ::testing::Test rmw_qos[1].lifespan.nsec = 5432u; // Add data to topic_cache - topic_cache.addTopic(instance_handler[0], "topic1", "type1", qos[0]); - topic_cache.addTopic(instance_handler[0], "topic2", "type2", qos[0]); - topic_cache.addTopic(instance_handler[1], "topic1", "type1", qos[1]); - topic_cache.addTopic(instance_handler[1], "topic2", "type1", qos[1]); + topic_cache.addTopic(participant_instance_handler[0], guid[0], "topic1", "type1", qos[0]); + topic_cache.addTopic(participant_instance_handler[0], guid[0], "topic2", "type2", qos[0]); + topic_cache.addTopic(participant_instance_handler[1], guid[1], "topic1", "type1", qos[1]); + topic_cache.addTopic(participant_instance_handler[1], guid[1], "topic2", "type1", qos[1]); } }; @@ -110,13 +112,14 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_types) // Verify that there are two entries for type1 EXPECT_EQ(std::count(topic_types.begin(), topic_types.end(), "type1"), 2); EXPECT_EQ(topic_types.at(0), "type1"); + EXPECT_EQ(topic_types.at(1), "type1"); // topic2 const auto & it2 = topic_type_map.find("topic2"); ASSERT_TRUE(it2 != topic_type_map.end()); // Verify that the object returned from the map is indeed the one expected topic_types = it2->second; - // Verify that there are two entries for type1 + // Verify that there are entries for type1 and type2 EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); } @@ -126,7 +129,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); // participant 1 - const auto & it = participant_topic_map.find(this->guid[0]); + const auto & it = participant_topic_map.find(this->participant_guid[0]); ASSERT_TRUE(it != participant_topic_map.end()); // Verify that the topic and respective types are present const auto & topic_type_map = it->second; @@ -142,7 +145,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); // participant 2 - const auto & it2 = participant_topic_map.find(this->guid[1]); + const auto & it2 = participant_topic_map.find(this->participant_guid[1]); ASSERT_TRUE(it2 != participant_topic_map.end()); // Verify that the topic and respective types are present const auto & topic_type_map2 = it2->second; @@ -161,12 +164,11 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) { const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); - auto expected_results = std::map>>(); - expected_results["topic1"].push_back(std::make_tuple(guid[0], "type1", rmw_qos[0])); - expected_results["topic1"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); - expected_results["topic2"].push_back(std::make_tuple(guid[0], "type2", rmw_qos[0])); - expected_results["topic2"].push_back(std::make_tuple(guid[1], "type1", rmw_qos[1])); + auto expected_results = std::map>(); + expected_results["topic1"].push_back({participant_guid[0], guid[0], "type1", rmw_qos[0]}); + expected_results["topic1"].push_back({participant_guid[1], guid[1], "type1", rmw_qos[1]}); + expected_results["topic2"].push_back({participant_guid[0], guid[0], "type2", rmw_qos[0]}); + expected_results["topic2"].push_back({participant_guid[1], guid[1], "type1", rmw_qos[1]}); for (const auto & result_it : expected_results) { const auto & topic_name = result_it.first; const auto & expected_topic_data = result_it.second; @@ -176,13 +178,15 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) // Verify that the topic has all the associated data const auto & topic_data = it->second; for (auto i = 0u; i < expected_topic_data.size(); i++) { + // PARTICIPANT GUID + EXPECT_EQ(topic_data.at(i).participant_guid, expected_topic_data.at(i).participant_guid); // GUID - EXPECT_EQ(std::get<0>(topic_data.at(i)), std::get<0>(expected_topic_data.at(i))); + EXPECT_EQ(topic_data.at(i).guid, expected_topic_data.at(i).guid); // TYPE - EXPECT_EQ(std::get<1>(topic_data.at(i)), std::get<1>(expected_topic_data.at(i))); + EXPECT_EQ(topic_data.at(i).topic_type, expected_topic_data.at(i).topic_type); // QOS - const auto & qos = std::get<2>(topic_data.at(i)); - const auto & expected_qos = std::get<2>(expected_topic_data.at(i)); + const auto & qos = topic_data.at(i).qos_profile; + const auto & expected_qos = expected_topic_data.at(i).qos_profile; EXPECT_EQ(qos.durability, expected_qos.durability); EXPECT_EQ(qos.reliability, expected_qos.reliability); EXPECT_EQ(qos.liveliness, expected_qos.liveliness); @@ -199,16 +203,18 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) TEST_F(TopicCacheTestFixture, test_topic_cache_add_topic) { // Add Topic - const bool did_add = this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", - "TestType", - this->qos[1]); + const bool did_add = + this->topic_cache.addTopic(this->participant_instance_handler[1], this->guid[1], "TestTopic", + "TestType", this->qos[1]); // Verify that the returned value was true EXPECT_TRUE(did_add); } TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) { - auto did_remove = this->topic_cache.removeTopic(this->instance_handler[0], "topic1", "type1"); + auto did_remove = + this->topic_cache.removeTopic(this->participant_instance_handler[0], this->guid[0], "topic1", + "type1"); // Assert that the return was true ASSERT_TRUE(did_remove); // Verify it is removed from TopicToTypes @@ -218,7 +224,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) EXPECT_EQ(std::count(topic_type_it->second.begin(), topic_type_it->second.end(), "type1"), 1); // Verify it is removed from ParticipantTopicMap const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); - const auto & participant_topic_it = participant_topic_map.find(this->guid[0]); + const auto & participant_topic_it = participant_topic_map.find(this->participant_guid[0]); ASSERT_TRUE(participant_topic_it != participant_topic_map.end()); const auto & p_topic_type_it = participant_topic_it->second.find("topic1"); ASSERT_TRUE(p_topic_type_it == participant_topic_it->second.end()); @@ -228,13 +234,14 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) ASSERT_TRUE(topic_data_it != topic_data_map.end()); EXPECT_EQ(topic_data_it->second.size(), 1u); - did_remove = this->topic_cache.removeTopic(this->instance_handler[1], "topic1", "type1"); + did_remove = this->topic_cache.removeTopic(this->participant_instance_handler[1], this->guid[1], + "topic1", "type1"); ASSERT_TRUE(did_remove); const auto & topic_type_map2 = this->topic_cache.getTopicToTypes(); const auto & topic_type_it2 = topic_type_map2.find("topic1"); ASSERT_TRUE(topic_type_it2 == topic_type_map2.end()); const auto & participant_topic_map2 = this->topic_cache.getParticipantToTopics(); - const auto & participant_topic_it2 = participant_topic_map2.find(this->guid[1]); + const auto & participant_topic_it2 = participant_topic_map2.find(this->participant_guid[1]); ASSERT_TRUE(participant_topic_it2 != participant_topic_map2.end()); const auto & p_topic_type_it2 = participant_topic_it2->second.find("topic1"); ASSERT_TRUE(p_topic_type_it2 == participant_topic_it2->second.end()); @@ -246,9 +253,11 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) TEST_F(TopicCacheTestFixture, test_topic_cache_remove_policy_element_does_not_exist) { // add topic - this->topic_cache.addTopic(this->instance_handler[1], "TestTopic", "TestType", this->qos[1]); + this->topic_cache.addTopic(this->participant_instance_handler[1], this->guid[1], "TestTopic", + "TestType", this->qos[1]); // Assert that the return was false - const auto did_remove = this->topic_cache.removeTopic(this->instance_handler[1], "NewTestTopic", + const auto did_remove = this->topic_cache.removeTopic(this->participant_instance_handler[1], + this->guid[1], "NewTestTopic", "TestType"); ASSERT_FALSE(did_remove); } From fea85a6abc09a40a8dab6a395dcf84d0f5dfadcf Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 26 Nov 2019 16:44:42 -0800 Subject: [PATCH 13/23] - Handled changes in headers from rmw - Cleaning up allocated memory on failed rmw_topic_info setters - Improvement in comments - TopicData.guid renamed to TopicData.entity_guid Signed-off-by: Jaison Titus --- rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp | 5 +- .../src/rmw_get_topic_info.cpp | 5 +- .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 3 +- .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 37 +++++------ .../src/rmw_get_topic_info.cpp | 63 ++++++++++++++++--- .../test/test_topic_cache.cpp | 2 +- 6 files changed, 84 insertions(+), 31 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp index daa2e1383..ccfe1234a 100644 --- a/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw/rmw.h" +#include "rmw/get_topic_info.h" +#include "rmw/topic_info_array.h" #include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" extern "C" { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp index 96b626e3e..4d9fc33e8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw/rmw.h" +#include "rmw/get_topic_info.h" +#include "rmw/topic_info_array.h" #include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" extern "C" { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index d54d2ce41..a188aff3c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -18,9 +18,10 @@ #include "./visibility_control.h" #include "rmw/error_handling.h" +#include "rmw/event.h" #include "rmw/rmw.h" +#include "rmw/topic_info_array.h" #include "rmw/types.h" -#include "rmw/event.h" #include "rmw/names_and_types.h" namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index f2176a016..d5655d049 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -42,7 +42,7 @@ typedef eprosima::fastrtps::rtps::GUID_t GUID_t; struct TopicData { GUID_t participant_guid; - GUID_t guid; /* publisher or subscription guid */ + GUID_t entity_guid; std::string topic_type; rmw_qos_profile_t qos_profile; }; @@ -150,16 +150,17 @@ class TopicCache /** * Add a topic based on discovery. * - * @param rtpsParticipantKey - * @param topic_name - * @param type_name - * @param dds_qos the dds qos policy of the participant + * @param rtpsParticipantKey the participant key of the discovered publisher or subscription + * @param entity_guid the guid of the publisher or subscription + * @param topic_name the topic name associated with the discovered publisher or subscription + * @param type_name the topic type associated with the discovered publisher or subscription + * @param dds_qos the dds qos policy of the discovered publisher or subscription * @return true if a change has been recorded */ template bool addTopic( const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, - const GUID_t & guid, /* publisher or subscription GUID */ + const GUID_t & entity_guid, const std::string & topic_name, const std::string & type_name, const T & dds_qos) @@ -167,12 +168,12 @@ class TopicCache initializeTopicDataMap(topic_name, topic_name_to_topic_data_); auto participant_guid = iHandle2GUID(rtpsParticipantKey); initializeParticipantMap(participant_to_topics_, participant_guid); - initializeTopicTypesMap(topic_name, participant_to_topics_[guid]); + initializeTopicTypesMap(topic_name, participant_to_topics_[entity_guid]); if (rcutils_logging_logger_is_enabled_for("rmw_fastrtps_shared_cpp", RCUTILS_LOG_SEVERITY_DEBUG)) { std::stringstream guid_stream; - guid_stream << guid; + guid_stream << entity_guid; RCUTILS_LOG_DEBUG_NAMED( "rmw_fastrtps_shared_cpp", "Adding topic '%s' with type '%s' for node '%s'", @@ -182,7 +183,7 @@ class TopicCache dds_qos_to_rmw_qos(dds_qos, &qos_profile); TopicData topic_data = { participant_guid, - guid, + entity_guid, type_name, qos_profile }; @@ -194,15 +195,15 @@ class TopicCache /** * Remove a topic based on discovery. * - * @param rtpsParticipantKey - * @param guid the guid of the publisher or subscription - * @param topic_name - * @param type_name + * @param rtpsParticipantKey the participant key of the publisher or subscription + * @param entity_guid the guid of the publisher or subscription + * @param topic_name the topic name associated with the publisher or subscription + * @param type_name the topic type associated with the publisher or subscription * @return true if a change has been recorded */ bool removeTopic( const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, - const eprosima::fastrtps::rtps::GUID_t & guid, + const eprosima::fastrtps::rtps::GUID_t & entity_guid, const std::string & topic_name, const std::string & type_name) { @@ -216,8 +217,9 @@ class TopicCache { auto & type_vec = topic_name_to_topic_data_[topic_name]; type_vec.erase(std::find_if(type_vec.begin(), type_vec.end(), - [type_name, guid](const auto & topic_data) { - return type_name.compare(topic_data.topic_type) == 0 && guid == topic_data.guid; + [type_name, entity_guid](const auto & topic_data) { + return type_name.compare(topic_data.topic_type) == 0 && + entity_guid == topic_data.entity_guid; })); if (type_vec.empty()) { topic_name_to_topic_data_.erase(topic_name); @@ -237,11 +239,10 @@ class TopicCache participant_to_topics_.erase(participant_guid); } } else { - RCUTILS_LOG_DEBUG_NAMED( + RCUTILS_LOG_ERROR_NAMED( "rmw_fastrtps_shared_cpp", "Unable to remove topic, does not exist '%s' with type '%s'", topic_name.c_str(), type_name.c_str()); - return false; } return true; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 9b37ce9bf..ca2ce580f 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -88,6 +88,27 @@ _get_topic_fqdns(const char * topic_name, bool no_mangle) return topic_fqdns; } +typedef rmw_ret_t (* rmw_topic_info_fini_func_t)( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator); + +void +_handle_topic_info_member_fini( + rmw_topic_info_t * topic_info, + std::string err_msg_title, + rcutils_allocator_t * allocator, + rmw_topic_info_fini_func_t topic_info_fini_member_func) +{ + std::string error_string = err_msg_title + "\n" + rmw_get_error_string().str; + rmw_reset_error(); + rmw_ret_t ret = topic_info_fini_member_func(topic_info, allocator); + if (ret != RMW_RET_OK) { + error_string += rmw_get_error_string().str; + rmw_reset_error(); + RMW_SET_ERROR_MSG(error_string.c_str()); + } +} + rmw_ret_t _set_rmw_topic_info( rmw_topic_info_t * topic_info, @@ -104,19 +125,19 @@ _set_rmw_topic_info( ); uint8_t rmw_gid[RMW_GID_STORAGE_SIZE]; memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(&rmw_gid, &topic_data.guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + memcpy(&rmw_gid, &topic_data.entity_guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); rmw_ret_t ret = rmw_topic_info_set_gid(topic_info, rmw_gid, sizeof(eprosima::fastrtps::rtps::GUID_t)); if (ret != RMW_RET_OK) { return ret; } - // set topic type - ret = rmw_topic_info_set_topic_type(topic_info, topic_data.topic_type.c_str(), allocator); + // set qos profile + ret = rmw_topic_info_set_qos_profile(topic_info, &topic_data.qos_profile); if (ret != RMW_RET_OK) { return ret; } - // set qos profile - ret = rmw_topic_info_set_qos_profile(topic_info, &topic_data.qos_profile); + // set topic type + ret = rmw_topic_info_set_topic_type(topic_info, topic_data.topic_type.c_str(), allocator); if (ret != RMW_RET_OK) { return ret; } @@ -124,9 +145,17 @@ _set_rmw_topic_info( if (topic_data.participant_guid == participant_guid) { ret = rmw_topic_info_set_node_name(topic_info, node_name, allocator); if (ret != RMW_RET_OK) { + _handle_topic_info_member_fini(topic_info, "Failed to set node_name", allocator, + rmw_topic_info_fini_topic_type); return ret; } ret = rmw_topic_info_set_node_namespace(topic_info, node_namespace, allocator); + if (ret != RMW_RET_OK) { + _handle_topic_info_member_fini(topic_info, "Failed to set node_namespace", allocator, + rmw_topic_info_fini_node_name); + _handle_topic_info_member_fini(topic_info, "", allocator, rmw_topic_info_fini_topic_type); + return ret; + } return ret; } // This means that this discovered participant is not associated with the passed node @@ -140,6 +169,8 @@ _set_rmw_topic_info( ret = rmw_topic_info_set_node_name(topic_info, "_NODE_NAME_UNKNOWN_", allocator); } if (ret != RMW_RET_OK) { + _handle_topic_info_member_fini(topic_info, "Failed to set node_name", allocator, + rmw_topic_info_fini_topic_type); return ret; } // set node namespace @@ -150,6 +181,11 @@ _set_rmw_topic_info( } else { ret = rmw_topic_info_set_node_namespace(topic_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); } + if (ret != RMW_RET_OK) { + _handle_topic_info_member_fini(topic_info, "Failed to set node_namespace", allocator, + rmw_topic_info_fini_node_name); + _handle_topic_info_member_fini(topic_info, "", allocator, rmw_topic_info_fini_topic_type); + } return ret; } @@ -201,7 +237,17 @@ _get_info_by_topic( slave_target, allocator); if (ret != RMW_RET_OK) { - RMW_SET_ERROR_MSG("Failed to create set_rmw_topic_info."); + // Free all the space allocated to the previous topic_infos + for (auto & tinfo : topic_info_vector) { + _handle_topic_info_member_fini(&tinfo, "", allocator, + rmw_topic_info_fini_node_namespace); + _handle_topic_info_member_fini(&tinfo, "", allocator, rmw_topic_info_fini_node_name); + _handle_topic_info_member_fini(&tinfo, "", allocator, rmw_topic_info_fini_topic_type); + } + std::string error_msg = "Failed to create set_rmw_topic_info: "; + error_msg += rmw_get_error_string().str; + rmw_reset_error(); + RMW_SET_ERROR_MSG(error_msg.c_str()); return ret; } // add rmw_topic_info_t to a vector @@ -214,7 +260,10 @@ _get_info_by_topic( auto count = topic_info_vector.size(); ret = rmw_topic_info_array_init_with_size(participants_info, count, allocator); if (ret != RMW_RET_OK) { - RMW_SET_ERROR_MSG("rmw_topic_info_array_init_with_size failed to allocate memory."); + rmw_error_string_t error_message = rmw_get_error_string(); + rmw_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "rmw_topic_info_array_init_with_size failed to allocate memory: %s", error_message.str); return RMW_RET_BAD_ALLOC; } for (auto i = 0u; i < count; i++) { diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index 094d9ab75..a22a0557a 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -181,7 +181,7 @@ TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) // PARTICIPANT GUID EXPECT_EQ(topic_data.at(i).participant_guid, expected_topic_data.at(i).participant_guid); // GUID - EXPECT_EQ(topic_data.at(i).guid, expected_topic_data.at(i).guid); + EXPECT_EQ(topic_data.at(i).entity_guid, expected_topic_data.at(i).entity_guid); // TYPE EXPECT_EQ(topic_data.at(i).topic_type, expected_topic_data.at(i).topic_type); // QOS From 1a4d9fafd3191dce197058d6b79b897b65a93fb3 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Wed, 27 Nov 2019 11:46:24 -0800 Subject: [PATCH 14/23] Better error handling. Signed-off-by: Jaison Titus --- .../src/rmw_get_topic_info.cpp | 43 ++++++------------- 1 file changed, 12 insertions(+), 31 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index ca2ce580f..93d7c9571 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -88,20 +88,14 @@ _get_topic_fqdns(const char * topic_name, bool no_mangle) return topic_fqdns; } -typedef rmw_ret_t (* rmw_topic_info_fini_func_t)( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator); - void -_handle_topic_info_member_fini( +_handle_topic_info_fini( rmw_topic_info_t * topic_info, - std::string err_msg_title, - rcutils_allocator_t * allocator, - rmw_topic_info_fini_func_t topic_info_fini_member_func) + rcutils_allocator_t * allocator) { - std::string error_string = err_msg_title + "\n" + rmw_get_error_string().str; + std::string error_string = rmw_get_error_string().str; rmw_reset_error(); - rmw_ret_t ret = topic_info_fini_member_func(topic_info, allocator); + rmw_ret_t ret = rmw_topic_info_fini(topic_info, allocator); if (ret != RMW_RET_OK) { error_string += rmw_get_error_string().str; rmw_reset_error(); @@ -145,15 +139,10 @@ _set_rmw_topic_info( if (topic_data.participant_guid == participant_guid) { ret = rmw_topic_info_set_node_name(topic_info, node_name, allocator); if (ret != RMW_RET_OK) { - _handle_topic_info_member_fini(topic_info, "Failed to set node_name", allocator, - rmw_topic_info_fini_topic_type); return ret; } ret = rmw_topic_info_set_node_namespace(topic_info, node_namespace, allocator); if (ret != RMW_RET_OK) { - _handle_topic_info_member_fini(topic_info, "Failed to set node_namespace", allocator, - rmw_topic_info_fini_node_name); - _handle_topic_info_member_fini(topic_info, "", allocator, rmw_topic_info_fini_topic_type); return ret; } return ret; @@ -169,8 +158,6 @@ _set_rmw_topic_info( ret = rmw_topic_info_set_node_name(topic_info, "_NODE_NAME_UNKNOWN_", allocator); } if (ret != RMW_RET_OK) { - _handle_topic_info_member_fini(topic_info, "Failed to set node_name", allocator, - rmw_topic_info_fini_topic_type); return ret; } // set node namespace @@ -181,11 +168,6 @@ _set_rmw_topic_info( } else { ret = rmw_topic_info_set_node_namespace(topic_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); } - if (ret != RMW_RET_OK) { - _handle_topic_info_member_fini(topic_info, "Failed to set node_namespace", allocator, - rmw_topic_info_fini_node_name); - _handle_topic_info_member_fini(topic_info, "", allocator, rmw_topic_info_fini_topic_type); - } return ret; } @@ -227,7 +209,7 @@ _get_info_by_topic( const auto & it = topic_name_to_data.find(topic_name); if (it != topic_name_to_data.end()) { for (const auto & data : it->second) { - rmw_topic_info_t topic_info; + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); rmw_ret_t ret = _set_rmw_topic_info( &topic_info, participant_guid, @@ -237,17 +219,12 @@ _get_info_by_topic( slave_target, allocator); if (ret != RMW_RET_OK) { + // Free topic_info + _handle_topic_info_fini(&topic_info, allocator); // Free all the space allocated to the previous topic_infos for (auto & tinfo : topic_info_vector) { - _handle_topic_info_member_fini(&tinfo, "", allocator, - rmw_topic_info_fini_node_namespace); - _handle_topic_info_member_fini(&tinfo, "", allocator, rmw_topic_info_fini_node_name); - _handle_topic_info_member_fini(&tinfo, "", allocator, rmw_topic_info_fini_topic_type); + _handle_topic_info_fini(&tinfo, allocator); } - std::string error_msg = "Failed to create set_rmw_topic_info: "; - error_msg += rmw_get_error_string().str; - rmw_reset_error(); - RMW_SET_ERROR_MSG(error_msg.c_str()); return ret; } // add rmw_topic_info_t to a vector @@ -264,6 +241,10 @@ _get_info_by_topic( rmw_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "rmw_topic_info_array_init_with_size failed to allocate memory: %s", error_message.str); + // Free all the space allocated to the previous topic_infos + for (auto & tinfo : topic_info_vector) { + _handle_topic_info_fini(&tinfo, allocator); + } return RMW_RET_BAD_ALLOC; } for (auto i = 0u; i < count; i++) { From 169f21856a18568a650ea789cd077bd17a0f95e8 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 10 Dec 2019 16:25:01 -0800 Subject: [PATCH 15/23] fix error handling in _handle_topic_info_fini() Signed-off-by: Miaofei --- .../src/rmw_get_topic_info.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 93d7c9571..e9354de9d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -93,12 +93,23 @@ _handle_topic_info_fini( rmw_topic_info_t * topic_info, rcutils_allocator_t * allocator) { - std::string error_string = rmw_get_error_string().str; + bool had_error = rmw_error_is_set(); + std::string error_string; + if (had_error) { + error_string = rmw_get_error_string().str; + } rmw_reset_error(); + rmw_ret_t ret = rmw_topic_info_fini(topic_info, allocator); if (ret != RMW_RET_OK) { - error_string += rmw_get_error_string().str; + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_cpp", + "rmw_topic_info_fini failed: %s", + rmw_get_error_string().str); rmw_reset_error(); + } + + if (had_error) { RMW_SET_ERROR_MSG(error_string.c_str()); } } @@ -245,7 +256,7 @@ _get_info_by_topic( for (auto & tinfo : topic_info_vector) { _handle_topic_info_fini(&tinfo, allocator); } - return RMW_RET_BAD_ALLOC; + return ret; } for (auto i = 0u; i < count; i++) { participants_info->info_array[i] = topic_info_vector.at(i); From 36e9718ceeb8daca43ae3fd125fcf72f57f997df Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 13 Dec 2019 19:04:15 -0800 Subject: [PATCH 16/23] address PR comments Signed-off-by: Miaofei --- .../include/rmw_fastrtps_shared_cpp/topic_cache.hpp | 11 ++++++----- rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp | 2 +- rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp | 3 ++- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index d5655d049..48e7d1886 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -29,10 +29,11 @@ #include "fastrtps/participant/Participant.h" #include "fastrtps/rtps/common/Guid.h" #include "fastrtps/rtps/common/InstanceHandle.h" -#include "qos.hpp" #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" +#include "qos.hpp" + typedef eprosima::fastrtps::rtps::GUID_t GUID_t; /** @@ -75,8 +76,8 @@ class TopicCache /** * Helper function to initialize an empty TopicData for a topic name. * - * @param topic_name the topic name for which the TopicNameToTopicData map should be initialised. - * @param topic_name_to_topic_data the map to initialise. + * @param topic_name the topic name for which the TopicNameToTopicData map should be initialized. + * @param topic_name_to_topic_data the map to initialize. */ void initializeTopicDataMap( const std::string & topic_name, @@ -90,8 +91,8 @@ class TopicCache /** * Helper function to initialize a topic vector. * - * @param topic_name the topic name for which the TopicToTypes map should be initialised. - * @param topic_to_types the map to initialise. + * @param topic_name the topic name for which the TopicToTypes map should be initialized. + * @param topic_to_types the map to initialize. */ void initializeTopicTypesMap(const std::string & topic_name, TopicToTypes & topic_to_types) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index e9354de9d..19e093e1d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -214,7 +214,7 @@ _get_info_by_topic( is_publisher ? slave_target->writer_topic_cache : slave_target->reader_topic_cache; { std::lock_guard guard(topic_cache.getMutex()); - auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); + const auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); std::vector topic_info_vector; for (const auto & topic_name : topic_fqdns) { const auto & it = topic_name_to_data.find(topic_name); diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index a22a0557a..588a74ffe 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -40,6 +40,7 @@ class TopicCacheTestFixture : public ::testing::Test InstanceHandle_t participant_instance_handler[2]; GUID_t participant_guid[2]; GUID_t guid[2]; + void SetUp() { // Create instance handlers @@ -102,7 +103,7 @@ class TopicCacheTestFixture : public ::testing::Test TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_types) { - const auto & topic_type_map = this->topic_cache.getTopicToTypes(); + const auto topic_type_map = this->topic_cache.getTopicToTypes(); // topic1 const auto & it = topic_type_map.find("topic1"); From 8152876c3970a20d2e0225c74752d6a99939bcaf Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 18 Dec 2019 10:56:12 -0800 Subject: [PATCH 17/23] address PR comments Signed-off-by: Miaofei --- .../include/rmw_fastrtps_shared_cpp/topic_cache.hpp | 2 +- rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index 48e7d1886..b252a4b6c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -38,7 +38,7 @@ typedef eprosima::fastrtps::rtps::GUID_t GUID_t; /** * A data structure that encapsulates all the data associated with a publisher - * or subscription by the topic it publishers or subscribers to + * or subscription by the topic it publishes or subscribes to */ struct TopicData { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 19e093e1d..580c425a7 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -23,9 +23,10 @@ #include "rmw/topic_info_array.h" #include "rmw/topic_info.h" +#include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" namespace rmw_fastrtps_shared_cpp { @@ -142,7 +143,8 @@ _set_rmw_topic_info( return ret; } // set topic type - ret = rmw_topic_info_set_topic_type(topic_info, topic_data.topic_type.c_str(), allocator); + std::string type_name = _demangle_if_ros_type(topic_data.topic_type); + ret = rmw_topic_info_set_topic_type(topic_info, type_name.c_str(), allocator); if (ret != RMW_RET_OK) { return ret; } @@ -217,7 +219,7 @@ _get_info_by_topic( const auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); std::vector topic_info_vector; for (const auto & topic_name : topic_fqdns) { - const auto & it = topic_name_to_data.find(topic_name); + const auto it = topic_name_to_data.find(topic_name); if (it != topic_name_to_data.end()) { for (const auto & data : it->second) { rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); From 642d9edad329140659670ee4e37dfe85e64af40d Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 19 Dec 2019 12:11:43 -0800 Subject: [PATCH 18/23] conditionally demangle type name Signed-off-by: Miaofei --- rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 580c425a7..8fb4b13a1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -122,6 +122,7 @@ _set_rmw_topic_info( const char * node_name, const char * node_namespace, TopicData topic_data, + bool no_mangle, ::ParticipantListener * slave_target, rcutils_allocator_t * allocator) { @@ -143,7 +144,8 @@ _set_rmw_topic_info( return ret; } // set topic type - std::string type_name = _demangle_if_ros_type(topic_data.topic_type); + std::string type_name = + no_mangle ? _demangle_if_ros_type(topic_data.topic_type) : topic_data.topic_type; ret = rmw_topic_info_set_topic_type(topic_info, type_name.c_str(), allocator); if (ret != RMW_RET_OK) { return ret; @@ -229,6 +231,7 @@ _get_info_by_topic( node_name, node_namespace, data, + no_mangle, slave_target, allocator); if (ret != RMW_RET_OK) { From 38dec2a5d94a2cbdf70a078ad92bb334d7705a6d Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 27 Dec 2019 19:24:26 -0800 Subject: [PATCH 19/23] reverse condition for demangling type name Signed-off-by: Miaofei --- rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index 8fb4b13a1..beceb1bed 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -145,7 +145,7 @@ _set_rmw_topic_info( } // set topic type std::string type_name = - no_mangle ? _demangle_if_ros_type(topic_data.topic_type) : topic_data.topic_type; + no_mangle ? topic_data.topic_type : _demangle_if_ros_type(topic_data.topic_type); ret = rmw_topic_info_set_topic_type(topic_info, type_name.c_str(), allocator); if (ret != RMW_RET_OK) { return ret; From c231f14f92759a898c05af27b2a56a97c44038ae Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 2 Jan 2020 10:39:19 -0800 Subject: [PATCH 20/23] properly initialize history and history depth in topic info Signed-off-by: Miaofei --- .../include/rmw_fastrtps_shared_cpp/topic_cache.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index b252a4b6c..62787ee55 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -180,7 +180,7 @@ class TopicCache "Adding topic '%s' with type '%s' for node '%s'", topic_name.c_str(), type_name.c_str(), guid_stream.str().c_str()); } - rmw_qos_profile_t qos_profile = rmw_qos_profile_t(); + rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; dds_qos_to_rmw_qos(dds_qos, &qos_profile); TopicData topic_data = { participant_guid, From a1a5e798d3766d77d677657ce64e3748748b914a Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 Jan 2020 16:06:41 -0800 Subject: [PATCH 21/23] address more PR comments Signed-off-by: Miaofei --- .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 43 ++++++++------- .../src/rmw_get_topic_info.cpp | 19 ++++++- .../src/rmw_node_info_and_types.cpp | 54 +++++++++---------- .../test/test_topic_cache.cpp | 1 + 4 files changed, 66 insertions(+), 51 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index 62787ee55..a6201a3fb 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -76,8 +75,8 @@ class TopicCache /** * Helper function to initialize an empty TopicData for a topic name. * - * @param topic_name the topic name for which the TopicNameToTopicData map should be initialized. - * @param topic_name_to_topic_data the map to initialize. + * \param topic_name the topic name for which the TopicNameToTopicData map should be initialized. + * \param topic_name_to_topic_data the map to initialize. */ void initializeTopicDataMap( const std::string & topic_name, @@ -91,8 +90,8 @@ class TopicCache /** * Helper function to initialize a topic vector. * - * @param topic_name the topic name for which the TopicToTypes map should be initialized. - * @param topic_to_types the map to initialize. + * \param topic_name the topic name for which the TopicToTypes map should be initialized. + * \param topic_to_types the map to initialize. */ void initializeTopicTypesMap(const std::string & topic_name, TopicToTypes & topic_to_types) { @@ -104,8 +103,8 @@ class TopicCache /** * Helper function to initialize the set inside a participant map. * - * @param map - * @param guid + * \param map + * \param guid */ void initializeParticipantMap( ParticipantTopicMap & map, @@ -118,7 +117,7 @@ class TopicCache public: /** - * @return a map of topic name to the vector of topic types used. + * \return a map of topic name to the vector of topic types used. */ const TopicToTypes getTopicToTypes() const { @@ -133,7 +132,7 @@ class TopicCache } /** - * @return a map of participant guid to the vector of topic names used. + * \return a map of participant guid to the vector of topic names used. */ const ParticipantTopicMap & getParticipantToTopics() const { @@ -141,7 +140,7 @@ class TopicCache } /** - * @return a map of topic name to a vector of GUID_t, type name and qos profile tuple. + * \return a map of topic name to a vector of GUID_t, type name and qos profile tuple. */ const TopicNameToTopicData & getTopicNameToTopicData() const { @@ -151,12 +150,12 @@ class TopicCache /** * Add a topic based on discovery. * - * @param rtpsParticipantKey the participant key of the discovered publisher or subscription - * @param entity_guid the guid of the publisher or subscription - * @param topic_name the topic name associated with the discovered publisher or subscription - * @param type_name the topic type associated with the discovered publisher or subscription - * @param dds_qos the dds qos policy of the discovered publisher or subscription - * @return true if a change has been recorded + * \param rtpsParticipantKey the participant key of the discovered publisher or subscription + * \param entity_guid the guid of the publisher or subscription + * \param topic_name the topic name associated with the discovered publisher or subscription + * \param type_name the topic type associated with the discovered publisher or subscription + * \param dds_qos the dds qos policy of the discovered publisher or subscription + * \return true if a change has been recorded */ template bool addTopic( @@ -196,11 +195,11 @@ class TopicCache /** * Remove a topic based on discovery. * - * @param rtpsParticipantKey the participant key of the publisher or subscription - * @param entity_guid the guid of the publisher or subscription - * @param topic_name the topic name associated with the publisher or subscription - * @param type_name the topic type associated with the publisher or subscription - * @return true if a change has been recorded + * \param rtpsParticipantKey the participant key of the publisher or subscription + * \param entity_guid the guid of the publisher or subscription + * \param topic_name the topic name associated with the publisher or subscription + * \param type_name the topic type associated with the publisher or subscription + * \return true if a change has been recorded */ bool removeTopic( const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, @@ -287,7 +286,7 @@ class LockedObject public: /** - * @return a reference to this object to lock. + * \return a reference to this object to lock. */ std::mutex & getMutex() const RCPPUTILS_TSA_RETURN_CAPABILITY(mutex_) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp index beceb1bed..58250ca5e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp @@ -123,6 +123,7 @@ _set_rmw_topic_info( const char * node_namespace, TopicData topic_data, bool no_mangle, + bool is_publisher, ::ParticipantListener * slave_target, rcutils_allocator_t * allocator) { @@ -130,11 +131,24 @@ _set_rmw_topic_info( sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." ); + rmw_ret_t ret; + // set endpoint type + if (is_publisher) { + ret = rmw_topic_info_set_endpoint_type(topic_info, RMW_ENDPOINT_PUBLISHER); + } else { + ret = rmw_topic_info_set_endpoint_type(topic_info, RMW_ENDPOINT_SUBSCRIPTION); + } + if (ret != RMW_RET_OK) { + return ret; + } + // set endpoint gid uint8_t rmw_gid[RMW_GID_STORAGE_SIZE]; memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); memcpy(&rmw_gid, &topic_data.entity_guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); - rmw_ret_t ret = rmw_topic_info_set_gid(topic_info, rmw_gid, - sizeof(eprosima::fastrtps::rtps::GUID_t)); + ret = rmw_topic_info_set_gid( + topic_info, + rmw_gid, + sizeof(eprosima::fastrtps::rtps::GUID_t)); if (ret != RMW_RET_OK) { return ret; } @@ -232,6 +246,7 @@ _get_info_by_topic( node_namespace, data, no_mangle, + is_publisher, slave_target, allocator); if (ret != RMW_RET_OK) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp index 65b01607a..37e70e9d9 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp @@ -47,12 +47,12 @@ constexpr char kLoggerTag[] = "rmw_fastrtps_shared_cpp"; /** * Get the guid that corresponds to the node and namespace. * - * @param node to discover other participants with - * @param node_name of the desired node - * @param node_namespace of the desired node - * @param guid [out] result - * @return RMW_RET_ERROR if unable to find guid - * @return RMW_RET_OK if guid is available + * \param node to discover other participants with + * \param node_name of the desired node + * \param node_namespace of the desired node + * \param guid [out] result + * \return RMW_RET_ERROR if unable to find guid + * \return RMW_RET_OK if guid is available */ rmw_ret_t __get_guid_by_name( const rmw_node_t * node, const char * node_name, @@ -96,9 +96,9 @@ rmw_ret_t __get_guid_by_name( /** * Validate the input data of node_info_and_types functions. * - * @return RMW_RET_INVALID_ARGUMENT for null input args - * @return RMW_RET_ERROR if identifier is not the same as the input node - * @return RMW_RET_OK if all input is valid + * \return RMW_RET_INVALID_ARGUMENT for null input args + * \return RMW_RET_ERROR if identifier is not the same as the input node + * \return RMW_RET_OK if all input is valid */ rmw_ret_t __validate_input( const char * identifier, @@ -145,10 +145,10 @@ rmw_ret_t __validate_input( * Get info from publisher and subscriber * Combined results from the two lists * - * @param topic_cache cache with topic information - * @param topics [out] resulting topics - * @param node_guid_ to find information for - * @param no_demangle true if demangling will not occur + * \param topic_cache cache with topic information + * \param topics [out] resulting topics + * \param node_guid_ to find information for + * \param no_demangle true if demangling will not occur */ void __accumulate_topics( @@ -183,11 +183,11 @@ __accumulate_topics( /** * Copy topic data to results * - * @param topics to copy over - * @param allocator to use - * @param no_demangle true if demangling will not occur - * @param topic_names_and_types [out] final rmw result - * @return RMW_RET_OK if successful + * \param topics to copy over + * \param allocator to use + * \param no_demangle true if demangling will not occur + * \param topic_names_and_types [out] final rmw result + * \return RMW_RET_OK if successful */ rmw_ret_t __copy_data_to_results( @@ -313,15 +313,15 @@ typedef std::function&(CustomParticipantInfo & pa /** * Get topic names and types for the specific node_name and node_namespace requested. * - * @param identifier corresponding to the input node - * @param node to use for discovery - * @param allocator for returned value - * @param node_name to search - * @param node_namespace to search - * @param no_demangle true if the topics should not be demangled - * @param retrieve_cache_func getter for topic cache - * @param topic_names_and_types result - * @return RMW_RET_OK if successful + * \param identifier corresponding to the input node + * \param node to use for discovery + * \param allocator for returned value + * \param node_name to search + * \param node_namespace to search + * \param no_demangle true if the topics should not be demangled + * \param retrieve_cache_func getter for topic cache + * \param topic_names_and_types result + * \return RMW_RET_OK if successful */ rmw_ret_t __rmw_get_topic_names_and_types_by_node( diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp index 588a74ffe..c5b6bf031 100644 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp @@ -11,6 +11,7 @@ // 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 #include #include From 354cae2809d1a65f304203369c7950b28d40981e Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 Jan 2020 23:44:42 -0800 Subject: [PATCH 22/23] rename *topic_info* to *topic_endpoint_info* Signed-off-by: Miaofei --- rmw_fastrtps_cpp/CMakeLists.txt | 2 +- ...fo.cpp => rmw_get_topic_endpoint_info.cpp} | 8 +- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 2 +- ...fo.cpp => rmw_get_topic_endpoint_info.cpp} | 8 +- rmw_fastrtps_shared_cpp/CMakeLists.txt | 2 +- .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 6 +- ...fo.cpp => rmw_get_topic_endpoint_info.cpp} | 86 +++++++++---------- 7 files changed, 57 insertions(+), 57 deletions(-) rename rmw_fastrtps_cpp/src/{rmw_get_topic_info.cpp => rmw_get_topic_endpoint_info.cpp} (88%) rename rmw_fastrtps_dynamic_cpp/src/{rmw_get_topic_info.cpp => rmw_get_topic_endpoint_info.cpp} (88%) rename rmw_fastrtps_shared_cpp/src/{rmw_get_topic_info.cpp => rmw_get_topic_endpoint_info.cpp} (71%) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 55211e715..45314ad1a 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -60,7 +60,7 @@ add_library(rmw_fastrtps_cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_implementation_identifier.cpp src/rmw_get_serialization_format.cpp - src/rmw_get_topic_info.cpp + src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp src/rmw_init.cpp src/rmw_node.cpp diff --git a/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_cpp/src/rmw_get_topic_endpoint_info.cpp similarity index 88% rename from rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp rename to rmw_fastrtps_cpp/src/rmw_get_topic_endpoint_info.cpp index ccfe1234a..9982d73a7 100644 --- a/rmw_fastrtps_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw/get_topic_info.h" -#include "rmw/topic_info_array.h" +#include "rmw/get_topic_endpoint_info.h" +#include "rmw/topic_endpoint_info_array.h" #include "rmw/types.h" #include "rmw_fastrtps_cpp/identifier.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -26,7 +26,7 @@ rmw_get_publishers_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * publishers_info) + rmw_topic_endpoint_info_array_t * publishers_info) { return rmw_fastrtps_shared_cpp::__rmw_get_publishers_info_by_topic( eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, publishers_info); @@ -38,7 +38,7 @@ rmw_get_subscriptions_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * subscriptions_info) + rmw_topic_endpoint_info_array_t * subscriptions_info) { return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic( eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info); diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 024d834d1..c64f4215b 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -64,7 +64,7 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_implementation_identifier.cpp src/rmw_get_serialization_format.cpp - src/rmw_get_topic_info.cpp + src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp src/rmw_init.cpp src/rmw_node.cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_endpoint_info.cpp similarity index 88% rename from rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp rename to rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_endpoint_info.cpp index 4d9fc33e8..d19492e45 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw/get_topic_info.h" -#include "rmw/topic_info_array.h" +#include "rmw/get_topic_endpoint_info.h" +#include "rmw/topic_endpoint_info_array.h" #include "rmw/types.h" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -26,7 +26,7 @@ rmw_get_publishers_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * publishers_info) + rmw_topic_endpoint_info_array_t * publishers_info) { return rmw_fastrtps_shared_cpp::__rmw_get_publishers_info_by_topic( eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, publishers_info); @@ -38,7 +38,7 @@ rmw_get_subscriptions_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * subscriptions_info) + rmw_topic_endpoint_info_array_t * subscriptions_info) { return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic( eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info); diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index ac239dd0e..a8ba7c2f5 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -56,7 +56,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_compare_gids_equal.cpp src/rmw_count.cpp src/rmw_get_gid_for_publisher.cpp - src/rmw_get_topic_info.cpp + src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp src/rmw_logging.cpp src/rmw_node.cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index a188aff3c..a73ce8fbf 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -20,7 +20,7 @@ #include "rmw/error_handling.h" #include "rmw/event.h" #include "rmw/rmw.h" -#include "rmw/topic_info_array.h" +#include "rmw/topic_endpoint_info_array.h" #include "rmw/types.h" #include "rmw/names_and_types.h" @@ -358,7 +358,7 @@ __rmw_get_publishers_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * publishers_info); + rmw_topic_endpoint_info_array_t * publishers_info); RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t @@ -368,7 +368,7 @@ __rmw_get_subscriptions_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * subscriptions_info); + rmw_topic_endpoint_info_array_t * subscriptions_info); } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp similarity index 71% rename from rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp rename to rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index 58250ca5e..a7ddb1bf9 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -20,8 +20,8 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "rmw/topic_info_array.h" -#include "rmw/topic_info.h" +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/topic_endpoint_info.h" #include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" @@ -37,7 +37,7 @@ _validate_params( const rmw_node_t * node, rcutils_allocator_t * allocator, const char * topic_name, - rmw_topic_info_array_t * participants_info) + rmw_topic_endpoint_info_array_t * participants_info) { if (!identifier) { RMW_SET_ERROR_MSG("null implementation identifier provided"); @@ -90,8 +90,8 @@ _get_topic_fqdns(const char * topic_name, bool no_mangle) } void -_handle_topic_info_fini( - rmw_topic_info_t * topic_info, +_handle_topic_endpoint_info_fini( + rmw_topic_endpoint_info_t * topic_endpoint_info, rcutils_allocator_t * allocator) { bool had_error = rmw_error_is_set(); @@ -101,11 +101,11 @@ _handle_topic_info_fini( } rmw_reset_error(); - rmw_ret_t ret = rmw_topic_info_fini(topic_info, allocator); + rmw_ret_t ret = rmw_topic_endpoint_info_fini(topic_endpoint_info, allocator); if (ret != RMW_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rmw_fastrtps_cpp", - "rmw_topic_info_fini failed: %s", + "rmw_topic_endpoint_info_fini failed: %s", rmw_get_error_string().str); rmw_reset_error(); } @@ -116,8 +116,8 @@ _handle_topic_info_fini( } rmw_ret_t -_set_rmw_topic_info( - rmw_topic_info_t * topic_info, +_set_rmw_topic_endpoint_info( + rmw_topic_endpoint_info_t * topic_endpoint_info, const GUID_t & participant_guid, const char * node_name, const char * node_namespace, @@ -134,9 +134,9 @@ _set_rmw_topic_info( rmw_ret_t ret; // set endpoint type if (is_publisher) { - ret = rmw_topic_info_set_endpoint_type(topic_info, RMW_ENDPOINT_PUBLISHER); + ret = rmw_topic_endpoint_info_set_endpoint_type(topic_endpoint_info, RMW_ENDPOINT_PUBLISHER); } else { - ret = rmw_topic_info_set_endpoint_type(topic_info, RMW_ENDPOINT_SUBSCRIPTION); + ret = rmw_topic_endpoint_info_set_endpoint_type(topic_endpoint_info, RMW_ENDPOINT_SUBSCRIPTION); } if (ret != RMW_RET_OK) { return ret; @@ -145,32 +145,32 @@ _set_rmw_topic_info( uint8_t rmw_gid[RMW_GID_STORAGE_SIZE]; memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); memcpy(&rmw_gid, &topic_data.entity_guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); - ret = rmw_topic_info_set_gid( - topic_info, + ret = rmw_topic_endpoint_info_set_gid( + topic_endpoint_info, rmw_gid, sizeof(eprosima::fastrtps::rtps::GUID_t)); if (ret != RMW_RET_OK) { return ret; } // set qos profile - ret = rmw_topic_info_set_qos_profile(topic_info, &topic_data.qos_profile); + ret = rmw_topic_endpoint_info_set_qos_profile(topic_endpoint_info, &topic_data.qos_profile); if (ret != RMW_RET_OK) { return ret; } // set topic type std::string type_name = no_mangle ? topic_data.topic_type : _demangle_if_ros_type(topic_data.topic_type); - ret = rmw_topic_info_set_topic_type(topic_info, type_name.c_str(), allocator); + ret = rmw_topic_endpoint_info_set_topic_type(topic_endpoint_info, type_name.c_str(), allocator); if (ret != RMW_RET_OK) { return ret; } // Check if this participant is the same as the node that is passed if (topic_data.participant_guid == participant_guid) { - ret = rmw_topic_info_set_node_name(topic_info, node_name, allocator); + ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, node_name, allocator); if (ret != RMW_RET_OK) { return ret; } - ret = rmw_topic_info_set_node_namespace(topic_info, node_namespace, allocator); + ret = rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, node_namespace, allocator); if (ret != RMW_RET_OK) { return ret; } @@ -182,9 +182,9 @@ _set_rmw_topic_info( // set node name const auto & d_name_it = slave_target->discovered_names.find(topic_data.participant_guid); if (d_name_it != slave_target->discovered_names.end()) { - ret = rmw_topic_info_set_node_name(topic_info, d_name_it->second.c_str(), allocator); + ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, d_name_it->second.c_str(), allocator); } else { - ret = rmw_topic_info_set_node_name(topic_info, "_NODE_NAME_UNKNOWN_", allocator); + ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, "_NODE_NAME_UNKNOWN_", allocator); } if (ret != RMW_RET_OK) { return ret; @@ -193,9 +193,9 @@ _set_rmw_topic_info( const auto & d_namespace_it = slave_target->discovered_namespaces.find(topic_data.participant_guid); if (d_namespace_it != slave_target->discovered_namespaces.end()) { - ret = rmw_topic_info_set_node_namespace(topic_info, d_namespace_it->second.c_str(), allocator); + ret = rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, d_namespace_it->second.c_str(), allocator); } else { - ret = rmw_topic_info_set_node_namespace(topic_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); + ret = rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); } return ret; } @@ -209,7 +209,7 @@ _get_info_by_topic( const char * topic_name, bool no_mangle, bool is_publisher, - rmw_topic_info_array_t * participants_info) + rmw_topic_endpoint_info_array_t * participants_info) { rmw_ret_t ret = _validate_params( identifier, @@ -233,14 +233,14 @@ _get_info_by_topic( { std::lock_guard guard(topic_cache.getMutex()); const auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); - std::vector topic_info_vector; + std::vector topic_endpoint_info_vector; for (const auto & topic_name : topic_fqdns) { const auto it = topic_name_to_data.find(topic_name); if (it != topic_name_to_data.end()) { for (const auto & data : it->second) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); - rmw_ret_t ret = _set_rmw_topic_info( - &topic_info, + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rmw_ret_t ret = _set_rmw_topic_endpoint_info( + &topic_endpoint_info, participant_guid, node_name, node_namespace, @@ -250,36 +250,36 @@ _get_info_by_topic( slave_target, allocator); if (ret != RMW_RET_OK) { - // Free topic_info - _handle_topic_info_fini(&topic_info, allocator); - // Free all the space allocated to the previous topic_infos - for (auto & tinfo : topic_info_vector) { - _handle_topic_info_fini(&tinfo, allocator); + // Free topic_endpoint_info + _handle_topic_endpoint_info_fini(&topic_endpoint_info, allocator); + // Free all the space allocated to the previous topic_endpoint_infos + for (auto & tinfo : topic_endpoint_info_vector) { + _handle_topic_endpoint_info_fini(&tinfo, allocator); } return ret; } - // add rmw_topic_info_t to a vector - topic_info_vector.push_back(topic_info); + // add rmw_topic_endpoint_info_t to a vector + topic_endpoint_info_vector.push_back(topic_endpoint_info); } } } - // add all the elements from the vector to rmw_topic_info_array_t - auto count = topic_info_vector.size(); - ret = rmw_topic_info_array_init_with_size(participants_info, count, allocator); + // add all the elements from the vector to rmw_topic_endpoint_info_array_t + auto count = topic_endpoint_info_vector.size(); + ret = rmw_topic_endpoint_info_array_init_with_size(participants_info, count, allocator); if (ret != RMW_RET_OK) { rmw_error_string_t error_message = rmw_get_error_string(); rmw_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "rmw_topic_info_array_init_with_size failed to allocate memory: %s", error_message.str); - // Free all the space allocated to the previous topic_infos - for (auto & tinfo : topic_info_vector) { - _handle_topic_info_fini(&tinfo, allocator); + "rmw_topic_endpoint_info_array_init_with_size failed to allocate memory: %s", error_message.str); + // Free all the space allocated to the previous topic_endpoint_infos + for (auto & tinfo : topic_endpoint_info_vector) { + _handle_topic_endpoint_info_fini(&tinfo, allocator); } return ret; } for (auto i = 0u; i < count; i++) { - participants_info->info_array[i] = topic_info_vector.at(i); + participants_info->info_array[i] = topic_endpoint_info_vector.at(i); } participants_info->count = count; } @@ -293,7 +293,7 @@ __rmw_get_publishers_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * publishers_info) + rmw_topic_endpoint_info_array_t * publishers_info) { return _get_info_by_topic( identifier, @@ -312,7 +312,7 @@ __rmw_get_subscriptions_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * subscriptions_info) + rmw_topic_endpoint_info_array_t * subscriptions_info) { return _get_info_by_topic( identifier, From 1fe66edc66c1d93735b88ebb7fbd668491fbc43f Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 Jan 2020 23:57:39 -0800 Subject: [PATCH 23/23] fix formatting Signed-off-by: Miaofei --- .../src/rmw_get_topic_endpoint_info.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index a7ddb1bf9..336e0359c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -170,7 +170,8 @@ _set_rmw_topic_endpoint_info( if (ret != RMW_RET_OK) { return ret; } - ret = rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, node_namespace, allocator); + ret = + rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, node_namespace, allocator); if (ret != RMW_RET_OK) { return ret; } @@ -182,9 +183,11 @@ _set_rmw_topic_endpoint_info( // set node name const auto & d_name_it = slave_target->discovered_names.find(topic_data.participant_guid); if (d_name_it != slave_target->discovered_names.end()) { - ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, d_name_it->second.c_str(), allocator); + ret = rmw_topic_endpoint_info_set_node_name( + topic_endpoint_info, d_name_it->second.c_str(), allocator); } else { - ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, "_NODE_NAME_UNKNOWN_", allocator); + ret = rmw_topic_endpoint_info_set_node_name( + topic_endpoint_info, "_NODE_NAME_UNKNOWN_", allocator); } if (ret != RMW_RET_OK) { return ret; @@ -193,9 +196,11 @@ _set_rmw_topic_endpoint_info( const auto & d_namespace_it = slave_target->discovered_namespaces.find(topic_data.participant_guid); if (d_namespace_it != slave_target->discovered_namespaces.end()) { - ret = rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, d_namespace_it->second.c_str(), allocator); + ret = rmw_topic_endpoint_info_set_node_namespace( + topic_endpoint_info, d_namespace_it->second.c_str(), allocator); } else { - ret = rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); + ret = rmw_topic_endpoint_info_set_node_namespace( + topic_endpoint_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); } return ret; } @@ -238,7 +243,8 @@ _get_info_by_topic( const auto it = topic_name_to_data.find(topic_name); if (it != topic_name_to_data.end()) { for (const auto & data : it->second) { - rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rmw_topic_endpoint_info_t topic_endpoint_info = + rmw_get_zero_initialized_topic_endpoint_info(); rmw_ret_t ret = _set_rmw_topic_endpoint_info( &topic_endpoint_info, participant_guid, @@ -271,7 +277,8 @@ _get_info_by_topic( rmw_error_string_t error_message = rmw_get_error_string(); rmw_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "rmw_topic_endpoint_info_array_init_with_size failed to allocate memory: %s", error_message.str); + "rmw_topic_endpoint_info_array_init_with_size failed to allocate memory: %s", + error_message.str); // Free all the space allocated to the previous topic_endpoint_infos for (auto & tinfo : topic_endpoint_info_vector) { _handle_topic_endpoint_info_fini(&tinfo, allocator);