Skip to content

Commit

Permalink
Updated with PR comments
Browse files Browse the repository at this point in the history
Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
  • Loading branch information
ivanpauno authored and wjwwood committed May 5, 2019
1 parent efd6cfb commit 78d2228
Show file tree
Hide file tree
Showing 11 changed files with 72 additions and 89 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -70,12 +70,12 @@ 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;
publisher->publish(*msg);
msg.uint8_value = i;
publisher->publish(msg);
rclcpp::spin_some(node);
loop_rate.sleep();
}
Expand Down
4 changes: 2 additions & 2 deletions test_communication/test/test_subscription_valid_data.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,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
8 changes: 0 additions & 8 deletions test_rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -319,30 +319,22 @@ if(BUILD_TESTING)
# Parameter tests single implementation
custom_executable(test_parameters_server_cpp "test/test_parameters_server.cpp")
custom_gtest_executable(test_remote_parameters_cpp "test/test_remote_parameters.cpp")
target_include_directories(test_remote_parameters_cpp
PUBLIC ${GTEST_INCLUDE_DIRS})
target_link_libraries(test_remote_parameters_cpp
${GTEST_LIBRARIES})

# Service tests single implementation
custom_executable(test_services_server_cpp "test/test_services_server.cpp")
custom_gtest_executable(test_services_client_cpp "test/test_services_client.cpp")
target_include_directories(test_services_client_cpp
PUBLIC ${GTEST_INCLUDE_DIRS})
target_link_libraries(test_services_client_cpp
${GTEST_LIBRARIES})

custom_executable(test_client_scope_server_cpp "test/test_client_scope_server.cpp")
custom_gtest_executable(test_client_scope_client_cpp "test/test_client_scope_client.cpp")
target_include_directories(test_client_scope_client_cpp
PUBLIC ${GTEST_INCLUDE_DIRS})
target_link_libraries(test_client_scope_client_cpp
${GTEST_LIBRARIES})

custom_executable(test_client_scope_consistency_server_cpp "test/test_client_scope_consistency_server.cpp")
custom_gtest_executable(test_client_scope_consistency_client_cpp "test/test_client_scope_consistency_client.cpp")
target_include_directories(test_client_scope_consistency_client_cpp
PUBLIC ${GTEST_INCLUDE_DIRS})
target_link_libraries(test_client_scope_consistency_client_cpp
${GTEST_LIBRARIES})
endif() # BUILD_TESTING
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
4 changes: 2 additions & 2 deletions test_rclcpp/test/test_avoid_ros_namespace_conventions_qos.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,9 +65,9 @@ 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);
publisher->publish(msg);
};
// call the test template
single_message_pub_sub_fixture<test_rclcpp::msg::UInt32>(
Expand Down
28 changes: 14 additions & 14 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,8 +83,8 @@ 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;
publisher->publish(*msg);
msg.data = 1;
publisher->publish(msg);
ASSERT_EQ(0, counter);

// wait for the first callback
Expand All @@ -107,14 +107,14 @@ TEST(CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), nominal_
executor.spin_node_some(node);
ASSERT_EQ(1, counter);

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

// while four messages have been published one callback should be triggered here
Expand Down Expand Up @@ -157,8 +157,8 @@ 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;
publisher->publish(*msg);
msg.data = 6;
publisher->publish(msg);

std::this_thread::sleep_for(std::chrono::milliseconds(25));

Expand Down
24 changes: 12 additions & 12 deletions test_rclcpp/test/test_multithreaded.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,17 +79,17 @@ 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;
pub->publish(*msg);
++msg.data;
pub->publish(msg);

// test spin_some
// Expectation: The message was published and all subscriptions fired the callback.
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 All @@ -131,7 +131,7 @@ static inline void multi_consumer_pub_sub_test(bool intra_process)
executor.cancel();
return;
}
pub->publish(*msg);
pub->publish(msg);
};
auto timer = node->create_wall_timer(std::chrono::milliseconds(1), publish_callback);

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
4 changes: 2 additions & 2 deletions test_rclcpp/test/test_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,9 +60,9 @@ 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);
publisher->publish(msg);
};
// call the test template
single_message_pub_sub_fixture<test_rclcpp::msg::UInt32>(
Expand Down
12 changes: 6 additions & 6 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,8 +56,8 @@ TEST(CLASSNAME(test_repeated_publisher_subscriber, RMW_IMPLEMENTATION), subscrip
fflush(stdout);
executor.spin_node_some(node);

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

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

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

std::this_thread::sleep_for(std::chrono::milliseconds(50));
printf("spin_node_some()\n");
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
Loading

0 comments on commit 78d2228

Please sign in to comment.