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

Commit

Permalink
GUID fix for node graph implementation (#255)
Browse files Browse the repository at this point in the history 
* fix node graph unit tests

* cleanup header #includes

* addressing comments in code review

* fix formatting issue caught by linter

* address comments from OSRF
  • Loading branch information
@mm318 @clalancette
mm318 authored and clalancette committed Jan 25, 2019
1 parent 0de412c commit 25287a6
Show file tree
Hide file tree
Showing 4 changed files with 59 additions and 177 deletions.
131 changes: 8 additions & 123 deletions rmw_opensplice_cpp/src/guid.hpp
View file
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
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
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

0 comments on commit 25287a6

Please sign in to comment.