Skip to content
This repository has been archived by the owner on Oct 7, 2021. It is now read-only.

GUID fix for node graph implementation #255

Merged
merged 5 commits into from
Jan 25, 2019
Merged

GUID fix for node graph implementation #255

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a1d7a1f
fix node graph unit tests
mm318 Dec 11, 2018
ca30481
cleanup header #includes
mm318 Dec 11, 2018
5b8efb4
addressing comments in code review
mm318 Dec 17, 2018
f1f7c0a
fix formatting issue caught by linter
mm318 Dec 22, 2018
cce3be5
address comments from OSRF
mm318 Jan 24, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
131 changes: 8 additions & 123 deletions rmw_opensplice_cpp/src/guid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,133 +17,18 @@

#include <dds_dcps.h>

#include <cstring>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <vector>

// TODO(ross-desmond): This should all be in opensplice
typedef char octet;

/**
* Structure to hold GUID information for DDS instances.
* http://www.eprosima.com/docs/fast-rtps/1.6.0/html/_guid_8h_source.html
*
*/
struct GuidPrefix_t
inline DDS::InstanceHandle_t DDS_BuiltinTopicKey_to_InstanceHandle(
DDS::BuiltinTopicKey_t builtinTopicKey)
{
static constexpr unsigned int kSize = 12;
octet value[kSize];

GuidPrefix_t()
{
memset(value, 0, kSize);
}

explicit GuidPrefix_t(octet guid[kSize])
{
memcpy(value, guid, kSize);
}

GuidPrefix_t(const GuidPrefix_t & g)
{
memcpy(value, g.value, kSize);
}

GuidPrefix_t(GuidPrefix_t && g)
{
memmove(value, g.value, kSize);
}

GuidPrefix_t & operator=(const GuidPrefix_t & guidpre)
{
memcpy(value, guidpre.value, kSize);
return *this;
}

GuidPrefix_t & operator=(GuidPrefix_t && guidpre)
{
memmove(value, guidpre.value, kSize);
return *this;
}

#ifndef DOXYGEN_SHOULD_SKIP_THIS_PUBLIC

bool operator==(const GuidPrefix_t & prefix) const
{
return memcmp(value, prefix.value, kSize) == 0;
}

bool operator!=(const GuidPrefix_t & prefix) const
{
return memcmp(value, prefix.value, kSize) != 0;
}

#endif
};
v_builtinTopicKey gid;

inline bool operator<(const GuidPrefix_t & g1, const GuidPrefix_t & g2)
{
for (uint8_t i = 0; i < GuidPrefix_t::kSize; ++i) {
if (g1.value[i] < g2.value[i]) {
return true;
} else if (g1.value[i] > g2.value[i]) {
return false;
}
}
return false;
}

inline std::ostream & operator<<(std::ostream & output, const GuidPrefix_t & guiP)
{
output << std::hex;
for (uint8_t i = 0; i < GuidPrefix_t::kSize - 1; ++i) {
output << static_cast<int>(guiP.value[i]) << ".";
}
output << static_cast<int>(guiP.value[GuidPrefix_t::kSize - 1]);
return output << std::dec;
}

// TODO(ross-desmond): check this with opensplice source code to ensure compatibility
/**
* Convert an instance handle to a guid.
*
* @param guid [out] the resulting guid
* @param domain_id to prepend to the guid
* @param instance_handle to append to the guid
*/
inline void DDS_InstanceHandle_to_GUID(
struct GuidPrefix_t * guid,
DDS::DomainId_t domain_id,
DDS::InstanceHandle_t instance_handle)
{
memcpy(guid->value, reinterpret_cast<char *>(&domain_id), sizeof(DDS::DomainId_t));
memcpy(guid->value + sizeof(DDS::DomainId_t),
reinterpret_cast<char *>(&instance_handle), sizeof(DDS::InstanceHandle_t));
}

inline void DDS_BuiltinTopicKey_to_GUID(
struct GuidPrefix_t * guid,
DDS::BuiltinTopicKey_t buitinTopicKey)
{
#if BIG_ENDIAN
/* Big Endian */
memcpy(guid->value, reinterpret_cast<octet *>(buitinTopicKey), GuidPrefix_t::kSize);
#else
/* Little Endian */
int i;
octet * topicKeyBuffer = reinterpret_cast<octet *>(buitinTopicKey);
for (i = 0; i < 3; ++i) {
octet * guidElement = &guid->value[i * 3];
octet * keyBufferElement = reinterpret_cast<octet *>(&buitinTopicKey[i * 3]);
guidElement[0] = keyBufferElement[2];
guidElement[1] = keyBufferElement[1];
guidElement[2] = keyBufferElement[0];
}
// the following logic came from copyInTopicKey() in opensplice source code
gid.systemId = builtinTopicKey[0];
gid.localId = builtinTopicKey[1];
gid.serial = builtinTopicKey[2];

#endif
return u_instanceHandleFromGID(gid);
}

#endif // GUID_HPP_
23 changes: 12 additions & 11 deletions rmw_opensplice_cpp/src/rmw_node_info_and_types.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ __is_node_match(
* @param node_info to discover nodes
* @param node_name to match
* @param node_namespace to match
* @param key [out] guid key that matches the node name and namespace
* @param key [out] key (an InstanceHandle) that matches the node name and namespace
*
* @return RMW_RET_OK if success, ERROR otherwise
*/
Expand All @@ -81,7 +81,7 @@ __get_key(
OpenSpliceStaticNodeInfo * node_info,
const char * node_name,
const char * node_namespace,
GuidPrefix_t & key)
DDS::InstanceHandle_t & key)
{
auto participant = node_info->participant;
if (!participant) {
Expand All @@ -93,8 +93,7 @@ __get_key(
auto dds_ret = participant->get_qos(dpqos);
// @todo: ross-desmond implement self discovery
if (dds_ret == DDS::RETCODE_OK && __is_node_match(dpqos.user_data, node_name, node_namespace)) {
DDS_InstanceHandle_to_GUID(&key,
node_info->participant->get_domain_id(), node_info->participant->get_instance_handle());
key = node_info->participant->get_instance_handle();
return RMW_RET_OK;
}

Expand Down Expand Up @@ -124,7 +123,7 @@ __get_key(
if (strcmp(node_name, name.c_str()) == 0 &&
strcmp(node_namespace, ns.c_str()) == 0)
{
DDS_BuiltinTopicKey_to_GUID(&key, pbtd.key);
key = DDS_BuiltinTopicKey_to_InstanceHandle(pbtd.key);
return RMW_RET_OK;
}
}
Expand Down Expand Up @@ -177,14 +176,15 @@ rmw_get_subscriber_names_and_types_by_node(
}

auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
GuidPrefix_t key;
DDS::InstanceHandle_t key;
auto get_guid_err = __get_key(node_info, node_name, node_namespace, key);
if (get_guid_err != RMW_RET_OK) {
return get_guid_err;
}
// combine publisher and subscriber information
std::map<std::string, std::set<std::string>> topics;
node_info->subscriber_listener->fill_topic_names_and_types_by_guid(no_demangle, topics, key);
node_info->subscriber_listener->fill_topic_names_and_types_by_participant(no_demangle, topics,
key);

rmw_ret_t rmw_ret;
rmw_ret = copy_topics_names_and_types(topics, allocator, no_demangle, topic_names_and_types);
Expand Down Expand Up @@ -217,15 +217,16 @@ rmw_get_publisher_names_and_types_by_node(
}

auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
GuidPrefix_t key;
DDS::InstanceHandle_t key;
auto get_guid_err = __get_key(node_info, node_name, node_namespace, key);
if (get_guid_err != RMW_RET_OK) {
return get_guid_err;
}

// combine publisher and subscriber information
std::map<std::string, std::set<std::string>> topics;
node_info->publisher_listener->fill_topic_names_and_types_by_guid(no_demangle, topics, key);
node_info->publisher_listener->fill_topic_names_and_types_by_participant(no_demangle, topics,
key);

rmw_ret_t rmw_ret;
rmw_ret = copy_topics_names_and_types(topics, allocator, no_demangle, topic_names_and_types);
Expand Down Expand Up @@ -257,15 +258,15 @@ rmw_get_service_names_and_types_by_node(
}

auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
GuidPrefix_t key;
DDS::InstanceHandle_t key;
auto get_guid_err = __get_key(node_info, node_name, node_namespace, key);
if (get_guid_err != RMW_RET_OK) {
return get_guid_err;
}

// combine publisher and subscriber information
std::map<std::string, std::set<std::string>> services;
node_info->subscriber_listener->fill_service_names_and_types_by_guid(services, key);
node_info->subscriber_listener->fill_service_names_and_types_by_participant(services, key);

rmw_ret_t rmw_ret;
rmw_ret = copy_services_to_names_and_types(services, allocator, service_names_and_types);
Expand Down
60 changes: 28 additions & 32 deletions rmw_opensplice_cpp/src/types.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,17 +114,17 @@ CustomDataReaderListener::fill_service_names_and_types(
}
}

void CustomDataReaderListener::fill_topic_names_and_types_by_guid(
void CustomDataReaderListener::fill_topic_names_and_types_by_participant(
bool no_demangle,
std::map<std::string, std::set<std::string>> & tnat,
GuidPrefix_t & participant_guid)
DDS::InstanceHandle_t & participant)
{
std::lock_guard<std::mutex> lock(mutex_);
const auto & map = topic_cache.getTopicTypesByGuid(participant_guid);
if (map.size() == 0) {
const auto & map = topic_cache.getTopicTypesByGuid(participant);
if (map.empty()) {
RCUTILS_LOG_DEBUG_NAMED(
"rmw_opensplice_cpp",
"No topics for participant_guid");
"No topics for participant");
return;
}
for (auto & it : map) {
Expand All @@ -137,16 +137,16 @@ void CustomDataReaderListener::fill_topic_names_and_types_by_guid(
}
}

void CustomDataReaderListener::fill_service_names_and_types_by_guid(
void CustomDataReaderListener::fill_service_names_and_types_by_participant(
std::map<std::string, std::set<std::string>> & services,
GuidPrefix_t & participant_guid)
DDS::InstanceHandle_t & participant)
{
std::lock_guard<std::mutex> lock(mutex_);
const auto & map = topic_cache.getTopicTypesByGuid(participant_guid);
if (map.size() == 0) {
const auto & map = topic_cache.getTopicTypesByGuid(participant);
if (map.empty()) {
RCUTILS_LOG_DEBUG_NAMED(
"rmw_opensplice_cpp",
"No services for participant_guid");
"No services for participant");
return;
}
for (auto & it : map) {
Expand Down Expand Up @@ -186,29 +186,29 @@ print_discovery_logging(
}

void CustomDataReaderListener::add_information(
const GuidPrefix_t & participant_guid,
const GuidPrefix_t & topic_guid,
const DDS::InstanceHandle_t & participant,
const DDS::InstanceHandle_t & topic,
const std::string & topic_name,
const std::string & topic_type,
const EndPointType endpoint_type)
{
topic_cache.addTopic(participant_guid, topic_guid, topic_name, topic_type);
topic_cache.addTopic(participant, topic, topic_name, topic_type);
if (print_discovery_logging_) {
print_discovery_logging("+", topic_name, topic_type, endpoint_type);
}
}

void CustomDataReaderListener::remove_information(
const GuidPrefix_t & topic_guid,
const DDS::InstanceHandle_t & topic,
const EndPointType endpoint_type)
{
if (print_discovery_logging_) {
TopicCache<GuidPrefix_t>::TopicInfo topic_info;
if (topic_cache.getTopic(topic_guid, topic_info)) {
TopicCache<DDS::InstanceHandle_t>::TopicInfo topic_info;
if (topic_cache.getTopic(topic, topic_info)) {
print_discovery_logging("-", topic_info.name, topic_info.type, endpoint_type);
}
}
topic_cache.removeTopic(topic_guid);
topic_cache.removeTopic(topic);
}

CustomPublisherListener::CustomPublisherListener(rmw_guard_condition_t * graph_guard_condition)
Expand Down Expand Up @@ -240,16 +240,15 @@ CustomPublisherListener::on_data_available(DDS::DataReader * reader)

for (DDS::ULong i = 0; i < data_seq.length(); ++i) {
std::string topic_name = "";
GuidPrefix_t topic_guid;
DDS_BuiltinTopicKey_to_GUID(&topic_guid, data_seq[i].key);
DDS::InstanceHandle_t topic = DDS_BuiltinTopicKey_to_InstanceHandle(data_seq[i].key);
if (info_seq[i].valid_data && info_seq[i].instance_state == DDS::ALIVE_INSTANCE_STATE) {
topic_name = data_seq[i].topic_name.in();
GuidPrefix_t participant_guid;
DDS_BuiltinTopicKey_to_GUID(&participant_guid, data_seq[i].participant_key);
add_information(participant_guid, topic_guid, topic_name,
DDS::InstanceHandle_t participant = DDS_BuiltinTopicKey_to_InstanceHandle(
data_seq[i].participant_key);
add_information(participant, topic, topic_name,
data_seq[i].type_name.in(), PublisherEP);
} else {
remove_information(topic_guid, PublisherEP);
remove_information(topic, PublisherEP);
}
}

Expand Down Expand Up @@ -291,23 +290,20 @@ CustomSubscriberListener::on_data_available(DDS::DataReader * reader)
}

for (DDS::ULong i = 0; i < data_seq.length(); ++i) {
std::string topic_name = "";
GuidPrefix_t topic_guid;

DDS_BuiltinTopicKey_to_GUID(&topic_guid, data_seq[i].key);
DDS::InstanceHandle_t topic = DDS_BuiltinTopicKey_to_InstanceHandle(data_seq[i].key);
if (info_seq[i].valid_data) {
std::string topic_name = "";
GuidPrefix_t participant_guid;
DDS_BuiltinTopicKey_to_GUID(&participant_guid, data_seq[i].participant_key);
DDS::InstanceHandle_t participant = DDS_BuiltinTopicKey_to_InstanceHandle(
data_seq[i].participant_key);
if (info_seq[i].instance_state == DDS::ALIVE_INSTANCE_STATE) {
topic_name = data_seq[i].topic_name.in();
add_information(participant_guid, topic_guid, topic_name,
add_information(participant, topic, topic_name,
data_seq[i].type_name.in(), SubscriberEP);
} else {
remove_information(topic_guid, SubscriberEP);
remove_information(topic, SubscriberEP);
}
} else {
remove_information(topic_guid, SubscriberEP);
remove_information(topic, SubscriberEP);
}
}

Expand Down
Loading