Skip to content

Commit

Permalink
Addressed PR comments
Browse files Browse the repository at this point in the history
Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
  • Loading branch information
ivanpauno committed Feb 19, 2019
1 parent cbd7240 commit 490a6f4
Show file tree
Hide file tree
Showing 5 changed files with 68 additions and 21 deletions.
10 changes: 6 additions & 4 deletions rclcpp/include/rclcpp/subscription.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,15 +145,16 @@ class SubscriptionBase
get_publisher_count() const;

protected:
std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
std::shared_ptr<rcl_subscription_t> subscription_handle_;
std::shared_ptr<rcl_node_t> node_handle_;

using IntraProcessManagerWeakPtr =
std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
bool use_intra_process_;
IntraProcessManagerWeakPtr weak_ipm_;
uint64_t intra_process_subscription_id_;

std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
std::shared_ptr<rcl_subscription_t> subscription_handle_;
std::shared_ptr<rcl_node_t> node_handle_;

private:
RCLCPP_DISABLE_COPY(SubscriptionBase)

Expand Down Expand Up @@ -326,6 +327,7 @@ class Subscription : public SubscriptionBase
get_intra_process_message_callback_ = get_message_callback;
matches_any_intra_process_publishers_ = matches_any_publisher_callback;
weak_ipm_ = weak_ipm;
use_intra_process_ = true;
}

/// Implemenation detail.
Expand Down
7 changes: 7 additions & 0 deletions rclcpp/src/rclcpp/publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -98,8 +98,15 @@ PublisherBase::~PublisherBase()
}

auto ipm = weak_ipm_.lock();

if (!use_intra_process_) {
return;
}
if (!ipm) {
// TODO(ivanpauno): should this raise an error?
RCLCPP_WARN(
rclcpp::get_logger("rclcpp"),
"Intra process manager died before than a publisher.");
return;
}
ipm->remove_publisher(intra_process_publisher_id_);
Expand Down
10 changes: 9 additions & 1 deletion rclcpp/src/rclcpp/subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,9 @@ SubscriptionBase::SubscriptionBase(
bool is_serialized)
: node_handle_(node_handle),
type_support_(type_support_handle),
is_serialized_(is_serialized)
is_serialized_(is_serialized),
use_intra_process_(false),
intra_process_subscription_id_(0)
{
auto custom_deletor = [node_handle](rcl_subscription_t * rcl_subs)
{
Expand Down Expand Up @@ -81,9 +83,15 @@ SubscriptionBase::SubscriptionBase(

SubscriptionBase::~SubscriptionBase()
{
if (!use_intra_process_) {
return;
}
auto ipm = weak_ipm_.lock();
if (!ipm) {
// TODO(ivanpauno): should this raise an error?
RCLCPP_WARN(
rclcpp::get_logger("rclcpp"),
"Intra process manager died before than a subscription.");
return;
}
ipm->remove_subscription(intra_process_subscription_id_);
Expand Down
29 changes: 20 additions & 9 deletions rclcpp/test/test_publisher_subscription_count_api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include <gtest/gtest.h>

#include <iostream>
#include <string>
#include <memory>

Expand All @@ -34,8 +35,15 @@ struct TestParameters
{
rclcpp::NodeOptions node_options[2];
uint64_t intraprocess_count_results[2];
std::string description;
};

std::ostream & operator<<(std::ostream & out, const TestParameters & params)
{
out << params.description;
return out;
}

class TestPublisherSubscriptionCount : public ::testing::TestWithParam<TestParameters>
{
public:
Expand Down Expand Up @@ -116,14 +124,15 @@ auto get_new_context()
TestParameters parameters[] = {
/*
Testing publisher subscription count api and internal process subscription count.
Two subscriptions in same using intra-process comm.
Two subscriptions in the same topic, both using intraprocess comm.
*/
{
{
rclcpp::NodeOptions().use_intra_process_comms(true),
rclcpp::NodeOptions().use_intra_process_comms(true)
},
{1u, 2u}
{1u, 2u},
"two_subscriptions_intraprocess_comm"
},
/*
Testing publisher subscription count api and internal process subscription count.
Expand All @@ -134,7 +143,8 @@ TestParameters parameters[] = {
rclcpp::NodeOptions().use_intra_process_comms(true),
rclcpp::NodeOptions().use_intra_process_comms(false)
},
{1u, 1u}
{1u, 1u},
"two_subscriptions_one_intraprocess_one_not"
},
/*
Testing publisher subscription count api and internal process subscription count.
Expand All @@ -145,7 +155,8 @@ TestParameters parameters[] = {
rclcpp::NodeOptions().use_intra_process_comms(true),
rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(true)
},
{1u, 1u}
{1u, 1u},
"two_subscriptions_in_two_contexts_with_intraprocess_comm"
},
/*
Testing publisher subscription count api and internal process subscription count.
Expand All @@ -156,12 +167,12 @@ TestParameters parameters[] = {
rclcpp::NodeOptions().use_intra_process_comms(false),
rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(false)
},
{0u, 0u}
{0u, 0u},
"two_subscriptions_in_two_contexts_without_intraprocess_comm"
}
};

// NOTE(ivanpauno): The extra comma is for avoiding a compiler warning, from GTEST.
INSTANTIATE_TEST_CASE_P(
TestWithDifferentNodeOptions,
TestPublisherSubscriptionCount,
::testing::ValuesIn(parameters), );
TestWithDifferentNodeOptions, TestPublisherSubscriptionCount,
::testing::ValuesIn(parameters),
::testing::PrintToStringParamName());
33 changes: 26 additions & 7 deletions rclcpp/test/test_subscription_publisher_count_api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include <gtest/gtest.h>

#include <iostream>
#include <string>
#include <memory>

Expand All @@ -24,7 +25,19 @@

using rcl_interfaces::msg::IntraProcessMessage;

class TestSubscriptionPublisherCount : public ::testing::TestWithParam<rclcpp::NodeOptions>
struct TestParameters
{
rclcpp::NodeOptions node_options;
std::string description;
};

std::ostream & operator<<(std::ostream & out, const TestParameters & params)
{
out << params.description;
return out;
}

class TestSubscriptionPublisherCount : public ::testing::TestWithParam<TestParameters>
{
public:
static void SetUpTestCase()
Expand All @@ -47,7 +60,7 @@ void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)

TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts)
{
rclcpp::NodeOptions node_options = GetParam();
rclcpp::NodeOptions node_options = GetParam().node_options;
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
"my_node",
"/ns",
Expand Down Expand Up @@ -84,21 +97,27 @@ auto get_new_context()
return context;
}

rclcpp::NodeOptions parameters[] = {
TestParameters parameters[] = {
/*
Testing subscription publisher count api.
One context.
*/
rclcpp::NodeOptions(),
{
rclcpp::NodeOptions(),
"one_context_test"
},
/*
Testing subscription publisher count api.
Two contexts.
*/
rclcpp::NodeOptions().context(get_new_context())
{
rclcpp::NodeOptions().context(get_new_context()),
"two_contexts_test"
}
};

// NOTE(ivanpauno): The extra comma is for avoiding a compiler warning, from GTEST.
INSTANTIATE_TEST_CASE_P(
TestWithDifferentNodeOptions,
TestSubscriptionPublisherCount,
::testing::ValuesIn(parameters), );
testing::ValuesIn(parameters),
testing::PrintToStringParamName());

0 comments on commit 490a6f4

Please sign in to comment.