Skip to content

Commit

Permalink
Centralize topic name creation logic and update to match FastRTPS 1.8…
Browse files Browse the repository at this point in the history
… API (#272)

Signed-off-by: Emerson Knapp <eknapp@amazon.com>
  • Loading branch information
nburek authored and dirk-thomas committed May 14, 2019
1 parent 6c1c91a commit 1d0d526
Show file tree
Hide file tree
Showing 11 changed files with 80 additions and 73 deletions.
17 changes: 5 additions & 12 deletions rmw_fastrtps_cpp/src/rmw_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -129,12 +130,8 @@ rmw_create_client(

subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = response_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
subscriberParam.topic.topicName = std::string(ros_service_response_prefix) + service_name;
} else {
subscriberParam.topic.topicName = service_name;
}
subscriberParam.topic.topicName += "Reply";
subscriberParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_response_prefix, service_name, "Reply");

if (!impl->leave_middleware_default_qos) {
publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE;
Expand All @@ -144,12 +141,8 @@ rmw_create_client(

publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = request_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
publisherParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name;
} else {
publisherParam.topic.topicName = service_name;
}
publisherParam.topic.topicName += "Request";
publisherParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_requester_prefix, service_name, "Request");

RCUTILS_LOG_DEBUG_NAMED(
"rmw_fastrtps_cpp",
Expand Down
7 changes: 2 additions & 5 deletions rmw_fastrtps_cpp/src/rmw_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -148,11 +149,7 @@ rmw_create_publisher(

publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
publisherParam.topic.topicName = std::string(ros_topic_prefix) + topic_name;
} else {
publisherParam.topic.topicName = topic_name;
}
publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name);

// 1 Heartbeat every 10ms
// publisherParam.times.heartbeatPeriod.seconds = 0;
Expand Down
17 changes: 5 additions & 12 deletions rmw_fastrtps_cpp/src/rmw_service.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@

#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -139,12 +140,8 @@ rmw_create_service(

subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = request_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
subscriberParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name;
} else {
subscriberParam.topic.topicName = service_name;
}
subscriberParam.topic.topicName += "Request";
subscriberParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_requester_prefix, service_name, "Request");

if (!impl->leave_middleware_default_qos) {
publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE;
Expand All @@ -154,12 +151,8 @@ rmw_create_service(

publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = response_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
publisherParam.topic.topicName = std::string(ros_service_response_prefix) + service_name;
} else {
publisherParam.topic.topicName = service_name;
}
publisherParam.topic.topicName += "Reply";
publisherParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_response_prefix, service_name, "Reply");

RCUTILS_LOG_DEBUG_NAMED(
"rmw_fastrtps_cpp",
Expand Down
7 changes: 2 additions & 5 deletions rmw_fastrtps_cpp/src/rmw_subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -150,11 +151,7 @@ rmw_create_subscription(

subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
subscriberParam.topic.topicName = std::string(ros_topic_prefix) + topic_name;
} else {
subscriberParam.topic.topicName = topic_name;
}
subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name);

if (!get_datareader_qos(*qos_policies, subscriberParam)) {
RMW_SET_ERROR_MSG("failed to get datareader qos");
Expand Down
17 changes: 5 additions & 12 deletions rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -136,12 +137,8 @@ rmw_create_client(

subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = response_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
subscriberParam.topic.topicName = std::string(ros_service_response_prefix) + service_name;
} else {
subscriberParam.topic.topicName = service_name;
}
subscriberParam.topic.topicName += "Reply";
subscriberParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_response_prefix, service_name, "Reply");

if (!impl->leave_middleware_default_qos) {
publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE;
Expand All @@ -151,12 +148,8 @@ rmw_create_client(

publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = request_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
publisherParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name;
} else {
publisherParam.topic.topicName = service_name;
}
publisherParam.topic.topicName += "Request";
publisherParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_requester_prefix, service_name, "Request");

RCUTILS_LOG_DEBUG_NAMED(
"rmw_fastrtps_dynamic_cpp",
Expand Down
7 changes: 2 additions & 5 deletions rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -144,11 +145,7 @@ rmw_create_publisher(

publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
publisherParam.topic.topicName = std::string(ros_topic_prefix) + topic_name;
} else {
publisherParam.topic.topicName = topic_name;
}
publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name);

// 1 Heartbeat every 10ms
// publisherParam.times.heartbeatPeriod.seconds = 0;
Expand Down
17 changes: 5 additions & 12 deletions rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@

#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"

Expand Down Expand Up @@ -146,12 +147,8 @@ rmw_create_service(

subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = request_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
subscriberParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name;
} else {
subscriberParam.topic.topicName = service_name;
}
subscriberParam.topic.topicName += "Request";
subscriberParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_requester_prefix, service_name, "Request");

if (!impl->leave_middleware_default_qos) {
publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE;
Expand All @@ -161,12 +158,8 @@ rmw_create_service(

publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
publisherParam.topic.topicDataType = response_type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
publisherParam.topic.topicName = std::string(ros_service_response_prefix) + service_name;
} else {
publisherParam.topic.topicName = service_name;
}
publisherParam.topic.topicName += "Reply";
publisherParam.topic.topicName = _create_topic_name(
qos_policies, ros_service_response_prefix, service_name, "Reply");

RCUTILS_LOG_DEBUG_NAMED(
"rmw_fastrtps_dynamic_cpp",
Expand Down
7 changes: 2 additions & 5 deletions rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp"
#include "rmw_fastrtps_shared_cpp/names.hpp"
#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp"
#include "rmw_fastrtps_shared_cpp/qos.hpp"
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
Expand Down Expand Up @@ -146,11 +147,7 @@ rmw_create_subscription(

subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY;
subscriberParam.topic.topicDataType = type_name;
if (!qos_policies->avoid_ros_namespace_conventions) {
subscriberParam.topic.topicName = std::string(ros_topic_prefix) + topic_name;
} else {
subscriberParam.topic.topicName = topic_name;
}
subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name);

if (!get_datareader_qos(*qos_policies, subscriberParam)) {
RMW_SET_ERROR_MSG("failed to get datareader qos");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -161,16 +161,15 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener
auto & topic_cache =
is_reader ? reader_topic_cache : writer_topic_cache;

auto fqdn = proxyData.topicName();
bool trigger;
{
std::lock_guard<std::mutex> guard(topic_cache.getMutex());
if (is_alive) {
trigger = topic_cache().addTopic(proxyData.RTPSParticipantKey(),
proxyData.topicName(), proxyData.typeName());
proxyData.topicName().to_string(), proxyData.typeName().to_string());
} else {
trigger = topic_cache().removeTopic(proxyData.RTPSParticipantKey(),
proxyData.topicName(), proxyData.typeName());
proxyData.topicName().to_string(), proxyData.typeName().to_string());
}
}
if (trigger) {
Expand Down
48 changes: 48 additions & 0 deletions rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
// 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__NAMES_HPP_
#define RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_

#include "fastrtps/utils/fixed_size_string.hpp"
#include "rmw/types.h"
#include "namespace_prefix.hpp"

/// Construct a topic name according to proper conventions.
/**
* \param[in] qos_profile The QoS profile for the topic.
* \param[in] prefix Required prefix for topic name.
* \param[in] base Required name of the topic.
* \param[in] suffix Optional suffix for topic name.
*/
inline
eprosima::fastrtps::string_255
_create_topic_name(
const rmw_qos_profile_t * qos_profile,
const char * prefix,
const char * base,
const char * suffix = nullptr)
{
std::ostringstream topicName;
if (!qos_profile->avoid_ros_namespace_conventions && prefix) {
topicName << prefix;
}
topicName << base;
if (suffix) {
topicName << suffix;
}
return topicName.str();
}

#endif // RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -64,12 +64,12 @@ __rmw_service_server_is_available(
}

auto pub_topic_name =
client_info->request_publisher_->getAttributes().topic.getTopicName();
client_info->request_publisher_->getAttributes().topic.getTopicName().to_string();

auto pub_fqdn = _demangle_if_ros_topic(pub_topic_name);

auto sub_topic_name =
client_info->response_subscriber_->getAttributes().topic.getTopicName();
client_info->response_subscriber_->getAttributes().topic.getTopicName().to_string();

auto sub_fqdn = _demangle_if_ros_topic(sub_topic_name);

Expand Down

0 comments on commit 1d0d526

Please sign in to comment.