Skip to content

Commit

Permalink
Corrected publish calls with shared_ptr signature (#348)
Browse files Browse the repository at this point in the history
* Corrected publish calls with shared_ptr signature

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

* Updated with PR comments

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

* Correct linter failure

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
  • Loading branch information
ivanpauno authored and wjwwood committed May 6, 2019
1 parent ccd02be commit 9cb12dc
Show file tree
Hide file tree
Showing 13 changed files with 56 additions and 64 deletions.
2 changes: 1 addition & 1 deletion test_communication/test/test_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ void publish(
// publish all messages one by one, shorter sleep between each message
while (rclcpp::ok() && message_index < messages.size()) {
printf("publishing message #%zu\n", message_index + 1);
publisher->publish(messages[message_index]);
publisher->publish(*messages[message_index]);
++message_index;
message_rate.sleep();
}
Expand Down
2 changes: 1 addition & 1 deletion test_communication/test/test_publisher_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void publish(
// publish all messages one by one, shorter sleep between each message
while (rclcpp::ok() && message_index < messages.size()) {
printf("publishing message #%zu\n", message_index + 1);
publisher->publish(messages[message_index]);
publisher->publish(*messages[message_index]);
++message_index;
message_rate.sleep();
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,11 +70,11 @@ TEST_F(CLASSNAME(TestMessageSerialization, RMW_IMPLEMENTATION), serialized_callb
auto publisher = node->create_publisher<test_msgs::msg::BasicTypes>(
"test_publisher_subscriber_serialized_topic");

auto msg = std::make_shared<test_msgs::msg::BasicTypes>();
test_msgs::msg::BasicTypes msg;

rclcpp::Rate loop_rate(10);
for (auto i = 0u; i < 10; ++i) {
msg->uint8_value = i;
msg.uint8_value = i;
publisher->publish(msg);
rclcpp::spin_some(node);
loop_rate.sleep();
Expand Down
5 changes: 3 additions & 2 deletions test_communication/test/test_subscription_valid_data.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <chrono>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"

Expand Down Expand Up @@ -52,9 +53,9 @@ int main(int argc, char ** argv)
// publish a few messages, all with data > 0
while (rclcpp::ok() && index <= 5) {
printf("publishing message #%u\n", index);
auto msg = std::make_shared<test_communication::msg::UInt32>();
auto msg = std::make_unique<test_communication::msg::UInt32>();
msg->data = index;
publisher->publish(msg);
publisher->publish(std::move(msg));
++index;
message_rate.sleep();
rclcpp::spin_some(node);
Expand Down
10 changes: 5 additions & 5 deletions test_rclcpp/test/pub_sub_fixtures.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ single_message_pub_sub_fixture(
)
> create_subscription_func,
std::function<
void(typename rclcpp::Publisher<MessageT>::SharedPtr, typename MessageT::SharedPtr)
void(typename rclcpp::Publisher<MessageT>::SharedPtr, MessageT)
> publish_func,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
std::function<void(rclcpp::executors::SingleThreadedExecutor &)> pre_subscription_hook = nullptr,
Expand All @@ -49,8 +49,8 @@ single_message_pub_sub_fixture(
auto publisher = node->create_publisher<MessageT>(
topic_name, custom_qos);

auto msg = std::make_shared<MessageT>();
msg->data = 0;
MessageT msg;
msg.data = 0;
rclcpp::executors::SingleThreadedExecutor executor;

// optionally call the pre subscription hook
Expand Down Expand Up @@ -78,9 +78,9 @@ single_message_pub_sub_fixture(
// this is necessary because sometimes the first message does not go through
// this is very common with Connext due to a race condition
size_t retry = 0;
msg->data = 0;
msg.data = 0;
while (retry < max_retries && counter == 0) {
msg->data++;
msg.data++;
// call custom publish function
publish_func(publisher, msg);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ TEST(
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
publisher->publish(msg);
};
Expand Down
16 changes: 8 additions & 8 deletions test_rclcpp/test/test_intra_process.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,8 @@ TEST(CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), nominal_
ASSERT_TRUE(message_info.from_intra_process);
};

auto msg = std::make_shared<test_rclcpp::msg::UInt32>();
msg->data = 0;
test_rclcpp::msg::UInt32 msg;
msg.data = 0;
rclcpp::executors::SingleThreadedExecutor executor;

{
Expand All @@ -83,7 +83,7 @@ TEST(CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), nominal_
// TODO(gerkey): fix nondeterministic startup behavior
std::this_thread::sleep_for(std::chrono::milliseconds(1));

msg->data = 1;
msg.data = 1;
publisher->publish(msg);
ASSERT_EQ(0, counter);

Expand All @@ -107,13 +107,13 @@ TEST(CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), nominal_
executor.spin_node_some(node);
ASSERT_EQ(1, counter);

msg->data = 2;
msg.data = 2;
publisher->publish(msg);
msg->data = 3;
msg.data = 3;
publisher->publish(msg);
msg->data = 4;
msg.data = 4;
publisher->publish(msg);
msg->data = 5;
msg.data = 5;
publisher->publish(msg);
ASSERT_EQ(1, counter);

Expand Down Expand Up @@ -157,7 +157,7 @@ TEST(CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), nominal_
}
// the subscriber goes out of scope and should be not receive any callbacks anymore

msg->data = 6;
msg.data = 6;
publisher->publish(msg);

std::this_thread::sleep_for(std::chrono::milliseconds(25));
Expand Down
20 changes: 10 additions & 10 deletions test_rclcpp/test/test_multithreaded.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,16 +79,16 @@ static inline void multi_consumer_pub_sub_test(bool intra_process)
int subscriptions_size = static_cast<int>(subscriptions.size());

executor.add_node(node);
auto msg = std::make_shared<test_rclcpp::msg::UInt32>();
msg->data = 0;
test_rclcpp::msg::UInt32 msg;
msg.data = 0;

// wait a moment for everything to initialize
test_rclcpp::wait_for_subscriber(node, node_topic_name);

// sanity check that no callbacks have fired
EXPECT_EQ(0, counter.load());

++msg->data;
++msg.data;
pub->publish(msg);

// test spin_some
Expand All @@ -110,19 +110,19 @@ static inline void multi_consumer_pub_sub_test(bool intra_process)

// reset counter
counter.store(0);
msg->data = 0;
msg.data = 0;

ASSERT_TRUE(std::numeric_limits<int>::max() > (5 * subscriptions.size()));
const int expected_count = static_cast<int>(5 * subscriptions.size());

std::mutex publish_mutex;
auto publish_callback = [
msg, &pub, &executor, &counter, &expected_count, &sleep_per_loop, &publish_mutex](
&msg, &pub, &executor, &counter, &expected_count, &sleep_per_loop, &publish_mutex](
rclcpp::TimerBase & timer) -> void
{
std::lock_guard<std::mutex> lock(publish_mutex);
++msg->data;
if (msg->data > 5) {
++msg.data;
if (msg.data > 5) {
timer.cancel();
// wait for the last callback to fire before cancelling
while (counter.load() != expected_count) {
Expand Down Expand Up @@ -278,7 +278,6 @@ static inline void multi_access_publisher(bool intra_process)
const size_t num_messages = 5 * std::min<size_t>(executor.get_number_of_threads(), 16);
auto pub = node->create_publisher<test_rclcpp::msg::UInt32>(node_topic_name, num_messages);
// callback groups?
auto msg = std::make_shared<test_rclcpp::msg::UInt32>();

std::atomic_uint subscription_counter(0);
auto sub_callback = [&subscription_counter](const test_rclcpp::msg::UInt32::SharedPtr msg)
Expand All @@ -302,9 +301,10 @@ static inline void multi_access_publisher(bool intra_process)
std::atomic_uint timer_counter(0);

auto timer_callback =
[&executor, &pub, &msg, &timer_counter, &subscription_counter, &num_messages](
[&executor, &pub, &timer_counter, &subscription_counter, &num_messages](
rclcpp::TimerBase & timer)
{
auto msg = std::make_unique<test_rclcpp::msg::UInt32>();
if (timer_counter.load() >= num_messages) {
timer.cancel();
// Wait for pending subscription callbacks to trigger.
Expand All @@ -316,7 +316,7 @@ static inline void multi_access_publisher(bool intra_process)
}
msg->data = ++timer_counter;
printf("Publishing message %u\n", timer_counter.load());
pub->publish(msg);
pub->publish(std::move(msg));
};
std::vector<rclcpp::TimerBase::SharedPtr> timers;
// timers will fire simultaneously in each thread
Expand Down
2 changes: 1 addition & 1 deletion test_rclcpp/test/test_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ TEST(CLASSNAME(test_publisher, RMW_IMPLEMENTATION), publish_with_const_reference
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
publisher->publish(msg);
};
Expand Down
8 changes: 4 additions & 4 deletions test_rclcpp/test/test_repeated_publisher_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,8 @@ TEST(CLASSNAME(test_repeated_publisher_subscriber, RMW_IMPLEMENTATION), subscrip
{
};

auto msg = std::make_shared<test_rclcpp::msg::UInt32>();
msg->data = 0;
test_rclcpp::msg::UInt32 msg;
msg.data = 0;
rclcpp::executors::SingleThreadedExecutor executor;

{
Expand All @@ -56,7 +56,7 @@ TEST(CLASSNAME(test_repeated_publisher_subscriber, RMW_IMPLEMENTATION), subscrip
fflush(stdout);
executor.spin_node_some(node);

msg->data = 1;
msg.data = 1;
publisher->publish(msg);

std::this_thread::sleep_for(std::chrono::milliseconds(50));
Expand All @@ -83,7 +83,7 @@ TEST(CLASSNAME(test_repeated_publisher_subscriber, RMW_IMPLEMENTATION), subscrip
fflush(stdout);
executor.spin_node_some(node);

msg->data = 2;
msg.data = 2;
publisher->publish(msg);

std::this_thread::sleep_for(std::chrono::milliseconds(50));
Expand Down
5 changes: 2 additions & 3 deletions test_rclcpp/test/test_spin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -199,7 +199,6 @@ TEST(CLASSNAME(test_spin, RMW_IMPLEMENTATION), cancel) {
auto node = rclcpp::Node::make_shared("cancel");
rclcpp::executors::SingleThreadedExecutor executor;
auto pub = node->create_publisher<test_rclcpp::msg::UInt32>("cancel", 10);
auto msg = std::make_shared<test_rclcpp::msg::UInt32>();

auto subscription_callback = [](test_rclcpp::msg::UInt32::ConstSharedPtr)
{
Expand All @@ -209,11 +208,11 @@ TEST(CLASSNAME(test_spin, RMW_IMPLEMENTATION), cancel) {
auto subscription = node->create_subscription<test_rclcpp::msg::UInt32>(
"cancel", subscription_callback, 10);

auto cancel_callback = [&executor, &pub, &msg]()
auto cancel_callback = [&executor, &pub]()
{
executor.cancel();
// Try to publish after canceling. The callback should never trigger.
pub->publish(msg);
pub->publish(test_rclcpp::msg::UInt32());
};
auto timer = node->create_wall_timer(std::chrono::milliseconds(5), cancel_callback);
executor.add_node(node);
Expand Down
42 changes: 17 additions & 25 deletions test_rclcpp/test/test_subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,8 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), subscription_and_spinning
sub_called.set_value();
};

auto msg = std::make_shared<test_rclcpp::msg::UInt32>();
msg->data = 0;
test_rclcpp::msg::UInt32 msg;
msg.data = 0;
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);

Expand All @@ -103,7 +103,7 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), subscription_and_spinning
executor.spin_some();
ASSERT_EQ(0, counter);

msg->data = 1;
msg.data = 1;
publisher->publish(msg);
ASSERT_EQ(0, counter);

Expand All @@ -120,13 +120,13 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), subscription_and_spinning
executor.spin_some();
ASSERT_EQ(1, counter);

msg->data = 2;
msg.data = 2;
publisher->publish(msg);
msg->data = 3;
msg.data = 3;
publisher->publish(msg);
msg->data = 4;
msg.data = 4;
publisher->publish(msg);
msg->data = 5;
msg.data = 5;
publisher->publish(msg);
ASSERT_EQ(1, counter);

Expand Down Expand Up @@ -160,7 +160,7 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), subscription_and_spinning
}
// the subscriber goes out of scope and should be not receive any callbacks anymore

msg->data = 6;
msg.data = 6;
publisher->publish(msg);

// check that no further callbacks have been invoked
Expand Down Expand Up @@ -198,11 +198,9 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), subscription_shared_ptr_c
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
// Create a ConstSharedPtr message to publish
test_rclcpp::msg::UInt32::ConstSharedPtr const_msg(msg);
publisher->publish(const_msg);
publisher->publish(msg);
};
// call the test template
single_message_pub_sub_fixture<test_rclcpp::msg::UInt32>(
Expand Down Expand Up @@ -246,11 +244,9 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION),
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
// Create a ConstSharedPtr message to publish
test_rclcpp::msg::UInt32::ConstSharedPtr const_msg(msg);
publisher->publish(const_msg);
publisher->publish(msg);
};
// call the test template
single_message_pub_sub_fixture<test_rclcpp::msg::UInt32>(
Expand Down Expand Up @@ -280,11 +276,9 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION),
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
// Create a ConstSharedPtr message to publish
test_rclcpp::msg::UInt32::ConstSharedPtr const_msg(msg);
publisher->publish(const_msg);
publisher->publish(msg);
};
// call the test template
single_message_pub_sub_fixture<test_rclcpp::msg::UInt32>(
Expand Down Expand Up @@ -318,7 +312,7 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), subscription_shared_ptr_c
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
publisher->publish(msg);
};
Expand Down Expand Up @@ -352,11 +346,9 @@ TEST(CLASSNAME(test_subscription, RMW_IMPLEMENTATION), spin_before_subscription)
auto publish_func =
[](
rclcpp::Publisher<test_rclcpp::msg::UInt32>::SharedPtr publisher,
test_rclcpp::msg::UInt32::SharedPtr msg)
test_rclcpp::msg::UInt32 msg)
{
// Create a ConstSharedPtr message to publish
test_rclcpp::msg::UInt32::ConstSharedPtr const_msg(msg);
publisher->publish(const_msg);
publisher->publish(msg);
};
// code for custom "pre subscription" hook
auto pre_subscription_hook =
Expand Down
2 changes: 1 addition & 1 deletion test_security/test/test_secure_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ int8_t attempt_publish(
// publish all messages one by one, shorter sleep between each message
while (rclcpp::ok() && message_index < messages.size()) {
printf("publishing message #%zu\n", message_index + 1);
publisher->publish(messages[message_index]);
publisher->publish(*messages[message_index]);
++message_index;
message_rate.sleep();
}
Expand Down

0 comments on commit 9cb12dc

Please sign in to comment.