Skip to content

Commit

Permalink
Fix namespaces (#196)
Browse files Browse the repository at this point in the history
* update rmw_fastrtps to not rely on namespace escalation fron Fast-RTPS

* revert spurious line change
  • Loading branch information
mikaelarguedas committed Apr 16, 2018
1 parent 85568b2 commit 698de9c
Show file tree
Hide file tree
Showing 19 changed files with 111 additions and 69 deletions.
4 changes: 2 additions & 2 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,9 +115,9 @@ class TypeSupport : public eprosima::fastrtps::TopicDataType

bool deserializeROSmessage(eprosima::fastcdr::FastBuffer * data, void * ros_message);

bool serialize(void * data, SerializedPayload_t * payload);
bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload);

bool deserialize(SerializedPayload_t * payload, void * data);
bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data);

std::function<uint32_t()> getSerializedSizeProvider(void * data);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -632,7 +632,7 @@ size_t TypeSupport<MembersType>::calculateMaxSerializedSize(
if (member->is_array_) {
array_size = member->array_size_;
// Whether it is a sequence.
if (array_size == 0 || member->is_upper_bound_) {
if (0 == array_size || member->is_upper_bound_) {
current_alignment += padding +
eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
}
Expand Down Expand Up @@ -738,7 +738,7 @@ void * TypeSupport<MembersType>::createData()

template<typename MembersType>
bool TypeSupport<MembersType>::serialize(
void * data, SerializedPayload_t * payload)
void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload)
{
assert(data);
assert(payload);
Expand All @@ -756,7 +756,9 @@ bool TypeSupport<MembersType>::serialize(
}

template<typename MembersType>
bool TypeSupport<MembersType>::deserialize(SerializedPayload_t * payload, void * data)
bool TypeSupport<MembersType>::deserialize(
eprosima::fastrtps::rtps::SerializedPayload_t * payload,
void * data)
{
assert(data);
assert(payload);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,10 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener
eprosima::fastrtps::SampleInfo_t sinfo;

if (sub->takeNextData(response.buffer_, &sinfo)) {
if (sinfo.sampleKind == ALIVE) {
if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) {
response.sample_identity_ = sinfo.related_sample_identity;

if (info_->writer_guid_ == response.sample_identity_.writer_guid()) {
if (response.sample_identity_.writer_guid() == info_->writer_guid_) {
std::lock_guard<std::mutex> lock(internalMutex_);

if (conditionMutex_ != nullptr) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,17 +43,19 @@ typedef struct CustomParticipantInfo
class ParticipantListener : public eprosima::fastrtps::ParticipantListener
{
public:
void onParticipantDiscovery(Participant *, ParticipantDiscoveryInfo info) override
void onParticipantDiscovery(
eprosima::fastrtps::Participant *,
eprosima::fastrtps::ParticipantDiscoveryInfo info) override
{
if (
info.rtps.m_status != DISCOVERED_RTPSPARTICIPANT &&
info.rtps.m_status != REMOVED_RTPSPARTICIPANT &&
info.rtps.m_status != DROPPED_RTPSPARTICIPANT)
info.rtps.m_status != eprosima::fastrtps::rtps::DISCOVERED_RTPSPARTICIPANT &&
info.rtps.m_status != eprosima::fastrtps::rtps::REMOVED_RTPSPARTICIPANT &&
info.rtps.m_status != eprosima::fastrtps::rtps::DROPPED_RTPSPARTICIPANT)
{
return;
}

if (DISCOVERED_RTPSPARTICIPANT == info.rtps.m_status) {
if (eprosima::fastrtps::rtps::DISCOVERED_RTPSPARTICIPANT == info.rtps.m_status) {
// ignore already known GUIDs
if (discovered_names.find(info.rtps.m_guid) == discovered_names.end()) {
auto map = rmw::impl::cpp::parse_key_value(info.rtps.m_userData);
Expand Down Expand Up @@ -90,7 +92,7 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener
return names;
}

std::map<GUID_t, std::string> discovered_names;
std::map<eprosima::fastrtps::rtps::GUID_t, std::string> discovered_names;
};

#endif // RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener
eprosima::fastrtps::SampleInfo_t sinfo;

if (sub->takeNextData(request.buffer_, &sinfo)) {
if (sinfo.sampleKind == ALIVE) {
if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) {
request.sample_identity_ = sinfo.sample_identity;

std::lock_guard<std::mutex> lock(internalMutex_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ class SubListener : public eprosima::fastrtps::SubscriberListener

void
onSubscriptionMatched(
eprosima::fastrtps::Subscriber * sub, eprosima::fastrtps::MatchingInfo & info)
eprosima::fastrtps::Subscriber * sub, eprosima::fastrtps::rtps::MatchingInfo & info)
{
(void)sub;
(void)info;
Expand Down
17 changes: 10 additions & 7 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/reader_info.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#include "fastrtps/rtps/reader/ReaderListener.h"
#include "fastrtps/rtps/reader/RTPSReader.h"

class ReaderInfo : public eprosima::fastrtps::ReaderListener
class ReaderInfo : public eprosima::fastrtps::rtps::ReaderListener
{
public:
ReaderInfo(
Expand All @@ -46,14 +46,17 @@ class ReaderInfo : public eprosima::fastrtps::ReaderListener
void
onNewCacheChangeAdded(
eprosima::fastrtps::rtps::RTPSReader *,
const eprosima::fastrtps::CacheChange_t * const change)
const eprosima::fastrtps::rtps::CacheChange_t * const change)
{
eprosima::fastrtps::rtps::ReaderProxyData proxyData;
if (change->kind == ALIVE) {
CDRMessage_t tempMsg(0);
if (eprosima::fastrtps::rtps::ALIVE == change->kind) {
eprosima::fastrtps::rtps::CDRMessage_t tempMsg(0);
tempMsg.wraps = true;
tempMsg.msg_endian = change->serializedPayload.encapsulation ==
PL_CDR_BE ? BIGEND : LITTLEEND;
if (PL_CDR_BE == change->serializedPayload.encapsulation) {
tempMsg.msg_endian = eprosima::fastrtps::rtps::BIGEND;
} else {
tempMsg.msg_endian = eprosima::fastrtps::rtps::LITTLEEND;
}
tempMsg.length = change->serializedPayload.length;
tempMsg.max_size = change->serializedPayload.max_size;
tempMsg.buffer = change->serializedPayload.data;
Expand All @@ -77,7 +80,7 @@ class ReaderInfo : public eprosima::fastrtps::ReaderListener

bool trigger = false;
mapmutex.lock();
if (change->kind == ALIVE) {
if (eprosima::fastrtps::rtps::ALIVE == change->kind) {
topicNtypes[fqdn].push_back(proxyData.typeName());
trigger = true;
} else {
Expand Down
17 changes: 10 additions & 7 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/writer_info.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@

#include "rmw/rmw.h"

class WriterInfo : public eprosima::fastrtps::ReaderListener
class WriterInfo : public eprosima::fastrtps::rtps::ReaderListener
{
public:
WriterInfo(
Expand All @@ -44,14 +44,17 @@ class WriterInfo : public eprosima::fastrtps::ReaderListener
void
onNewCacheChangeAdded(
eprosima::fastrtps::rtps::RTPSReader *,
const eprosima::fastrtps::CacheChange_t * const change)
const eprosima::fastrtps::rtps::CacheChange_t * const change)
{
eprosima::fastrtps::rtps::WriterProxyData proxyData;
if (change->kind == ALIVE) {
eprosima::fastrtps::CDRMessage_t tempMsg(0);
if (eprosima::fastrtps::rtps::ALIVE == change->kind) {
eprosima::fastrtps::rtps::CDRMessage_t tempMsg(0);
tempMsg.wraps = true;
tempMsg.msg_endian = change->serializedPayload.encapsulation ==
PL_CDR_BE ? BIGEND : LITTLEEND;
if (PL_CDR_BE == change->serializedPayload.encapsulation) {
tempMsg.msg_endian = eprosima::fastrtps::rtps::BIGEND;
} else {
tempMsg.msg_endian = eprosima::fastrtps::rtps::LITTLEEND;
}
tempMsg.length = change->serializedPayload.length;
tempMsg.max_size = change->serializedPayload.max_size;
tempMsg.buffer = change->serializedPayload.data;
Expand All @@ -75,7 +78,7 @@ class WriterInfo : public eprosima::fastrtps::ReaderListener

bool trigger = false;
mapmutex.lock();
if (change->kind == ALIVE) {
if (eprosima::fastrtps::rtps::ALIVE == change->kind) {
topicNtypes[fqdn].push_back(proxyData.typeName());
trigger = true;
} else {
Expand Down
4 changes: 2 additions & 2 deletions rmw_fastrtps_cpp/src/assign_partitions.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,13 +44,13 @@ _assign_partitions_to_attributes(
RMW_SET_ERROR_MSG(rcutils_get_error_string_safe());
return ret;
}
if (name_tokens.size == 1) {
if (1 == name_tokens.size) {
if (!avoid_ros_namespace_conventions) {
attributes->qos.m_partition.push_back(prefix);
}
attributes->topic.topicName = name_tokens.data[0];
ret = RCUTILS_RET_OK;
} else if (name_tokens.size == 2) {
} else if (2 == name_tokens.size) {
std::string partition;
if (avoid_ros_namespace_conventions) {
// no prefix to be used, just assign the user's namespace
Expand Down
6 changes: 3 additions & 3 deletions rmw_fastrtps_cpp/src/demangle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ _demangle_service_from_topic(const std::string & topic_name)
break;
}
}
if (suffix_position == std::string::npos) {
if (std::string::npos == suffix_position) {
RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_cpp",
"service topic has prefix but no suffix"
", report this: '%s'", topic_name.c_str())
Expand All @@ -108,7 +108,7 @@ _demangle_service_type_only(const std::string & dds_type_name)
{
std::string ns_substring = "::srv::dds_::";
size_t ns_substring_position = dds_type_name.find(ns_substring);
if (ns_substring_position == std::string::npos) {
if (std::string::npos == ns_substring_position) {
// not a ROS service type
return "";
}
Expand All @@ -131,7 +131,7 @@ _demangle_service_type_only(const std::string & dds_type_name)
break;
}
}
if (suffix_position == std::string::npos) {
if (std::string::npos == suffix_position) {
RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_cpp",
"service type contains '::srv::dds_::' but does not have a suffix"
", report this: '%s'", dds_type_name.c_str())
Expand Down
4 changes: 2 additions & 2 deletions rmw_fastrtps_cpp/src/qos.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ get_datareader_qos(
// ensure the history depth is at least the requested queue size
assert(sattr.topic.historyQos.depth >= 0);
if (
sattr.topic.historyQos.kind == eprosima::fastrtps::KEEP_LAST_HISTORY_QOS &&
eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == sattr.topic.historyQos.kind &&
static_cast<size_t>(sattr.topic.historyQos.depth) < qos_policies.depth)
{
if (qos_policies.depth > (std::numeric_limits<int32_t>::max)()) {
Expand Down Expand Up @@ -144,7 +144,7 @@ get_datawriter_qos(
// ensure the history depth is at least the requested queue size
assert(pattr.topic.historyQos.depth >= 0);
if (
pattr.topic.historyQos.kind == eprosima::fastrtps::KEEP_LAST_HISTORY_QOS &&
eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == pattr.topic.historyQos.kind &&
static_cast<size_t>(pattr.topic.historyQos.depth) < qos_policies.depth)
{
if (qos_policies.depth > (std::numeric_limits<int32_t>::max)()) {
Expand Down
20 changes: 13 additions & 7 deletions rmw_fastrtps_cpp/src/rmw_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,10 @@
#include "rmw_fastrtps_cpp/custom_client_info.hpp"
#include "rmw_fastrtps_cpp/custom_participant_info.hpp"

using Domain = eprosima::fastrtps::Domain;
using Participant = eprosima::fastrtps::Participant;
using TopicDataType = eprosima::fastrtps::TopicDataType;

extern "C"
{
rmw_client_t *
Expand Down Expand Up @@ -84,8 +88,8 @@ rmw_create_client(
}

CustomClientInfo * info = nullptr;
SubscriberAttributes subscriberParam;
PublisherAttributes publisherParam;
eprosima::fastrtps::SubscriberAttributes subscriberParam;
eprosima::fastrtps::PublisherAttributes publisherParam;
rmw_client_t * rmw_client = nullptr;

info = new CustomClientInfo();
Expand Down Expand Up @@ -121,9 +125,10 @@ rmw_create_client(
_register_type(participant, info->response_type_support_, info->typesupport_identifier_);
}

subscriberParam.topic.topicKind = NO_KEY;
subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = response_type_name;
subscriberParam.historyMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
subscriberParam.historyMemoryPolicy =
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
rcutils_ret_t ret = _assign_partitions_to_attributes(
service_name, ros_service_response_prefix,
qos_policies->avoid_ros_namespace_conventions, &subscriberParam);
Expand All @@ -133,10 +138,11 @@ rmw_create_client(
}
subscriberParam.topic.topicName += "Reply";

publisherParam.topic.topicKind = NO_KEY;
publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = request_type_name;
publisherParam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE;
publisherParam.historyMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE;
publisherParam.historyMemoryPolicy =
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
ret = _assign_partitions_to_attributes(
service_name, ros_service_requester_prefix,
qos_policies->avoid_ros_namespace_conventions, &publisherParam);
Expand Down
8 changes: 7 additions & 1 deletion rmw_fastrtps_cpp/src/rmw_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,11 @@
#include "rmw_fastrtps_cpp/identifier.hpp"
#include "rmw_fastrtps_cpp/custom_participant_info.hpp"

using Domain = eprosima::fastrtps::Domain;
using Participant = eprosima::fastrtps::Participant;
using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes;
using StatefulReader = eprosima::fastrtps::rtps::StatefulReader;

extern "C"
{
rmw_node_t *
Expand Down Expand Up @@ -246,7 +251,8 @@ rmw_create_node(
std::array<std::string, 3> security_files_paths;

if (get_security_file_paths(security_files_paths, security_options->security_root_path)) {
PropertyPolicy property_policy;
eprosima::fastrtps::rtps::PropertyPolicy property_policy;
using Property = eprosima::fastrtps::rtps::Property;
property_policy.properties().emplace_back(
Property("dds.sec.auth.plugin", "builtin.PKI-DH"));
property_policy.properties().emplace_back(
Expand Down
2 changes: 2 additions & 0 deletions rmw_fastrtps_cpp/src/rmw_node_names.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@
#include "rmw_fastrtps_cpp/identifier.hpp"
#include "rmw_fastrtps_cpp/custom_participant_info.hpp"

using Participant = eprosima::fastrtps::Participant;

extern "C"
{
rmw_ret_t
Expand Down
22 changes: 14 additions & 8 deletions rmw_fastrtps_cpp/src/rmw_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@
#include "rmw_fastrtps_cpp/custom_publisher_info.hpp"
#include "type_support_common.hpp"

using Domain = eprosima::fastrtps::Domain;
using Participant = eprosima::fastrtps::Participant;
using TopicDataType = eprosima::fastrtps::TopicDataType;

extern "C"
{
rmw_publisher_t *
Expand Down Expand Up @@ -79,8 +83,8 @@ rmw_create_publisher(

CustomPublisherInfo * info = nullptr;
rmw_publisher_t * rmw_publisher = nullptr;
PublisherAttributes publisherParam;
const GUID_t * guid = nullptr;
eprosima::fastrtps::PublisherAttributes publisherParam;
const eprosima::fastrtps::rtps::GUID_t * guid = nullptr;

// Load default XML profile.
Domain::getDefaultPublisherAttributes(publisherParam);
Expand All @@ -99,25 +103,27 @@ rmw_create_publisher(
_register_type(participant, info->type_support_, info->typesupport_identifier_);
}

publisherParam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE;
publisherParam.historyMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
publisherParam.topic.topicKind = NO_KEY;
publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE;
publisherParam.historyMemoryPolicy =
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = type_name;
rcutils_ret_t ret = _assign_partitions_to_attributes(
topic_name, ros_topic_prefix, qos_policies->avoid_ros_namespace_conventions, &publisherParam);
topic_name, ros_topic_prefix,
qos_policies->avoid_ros_namespace_conventions, &publisherParam);
if (ret != RCUTILS_RET_OK) {
// error msg already set
goto fail;
}

#if HAVE_SECURITY
// see if our participant has a security property set
if (eprosima::fastrtps::PropertyPolicyHelper::find_property(
if (eprosima::fastrtps::rtps::PropertyPolicyHelper::find_property(
participant->getAttributes().rtps.properties,
std::string("dds.sec.crypto.plugin")))
{
// set the encryption property on the publisher
PropertyPolicy publisher_property_policy;
eprosima::fastrtps::rtps::PropertyPolicy publisher_property_policy;
publisher_property_policy.properties().emplace_back(
"rtps.endpoint.submessage_protection_kind", "ENCRYPT");
publisher_property_policy.properties().emplace_back(
Expand Down
Loading

0 comments on commit 698de9c

Please sign in to comment.