Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
57 changes: 37 additions & 20 deletions rclcpp/test/rclcpp/executors/test_executors.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,28 +41,38 @@ template<typename T>
class TestExecutors : public ::testing::Test
{
public:
void SetUp()
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
node = std::make_shared<rclcpp::Node>("node", "ns");
}

callback_count = 0;
std::stringstream topic_name;
static void TearDownTestCase()
{
rclcpp::shutdown();
}

void SetUp()
{
const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
topic_name << "topic_" << test_info->test_case_name() << "_" << test_info->name();
std::stringstream test_name;
test_name << test_info->test_case_name() << "_" << test_info->name();
node = std::make_shared<rclcpp::Node>("node", test_name.str());

publisher = node->create_publisher<std_msgs::msg::Empty>(topic_name.str(), rclcpp::QoS(10));
callback_count = 0;

const std::string topic_name = std::string("topic_") + test_name.str();
publisher = node->create_publisher<std_msgs::msg::Empty>(topic_name, rclcpp::QoS(10));
auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;};
subscription =
node->create_subscription<std_msgs::msg::Empty>(
topic_name.str(), rclcpp::QoS(10), std::move(callback));
topic_name, rclcpp::QoS(10), std::move(callback));
}

void TearDown()
{
if (rclcpp::ok()) {
rclcpp::shutdown();
}
publisher.reset();
subscription.reset();
node.reset();
}

rclcpp::Node::SharedPtr node;
Expand Down Expand Up @@ -147,7 +157,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());});

std::this_thread::sleep_for(50ms);
rclcpp::shutdown();
executor.cancel();
spinner.join();
}

Expand All @@ -158,6 +168,7 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
ExecutorType executor2;
EXPECT_NO_THROW(executor1.add_node(this->node));
EXPECT_THROW(executor2.add_node(this->node), std::runtime_error);
executor1.remove_node(this->node, true);
}

// Check simple spin example
Expand All @@ -172,15 +183,15 @@ TYPED_TEST(TestExecutors, spinWithTimer) {
std::thread spinner([&]() {executor.spin();});

auto start = std::chrono::steady_clock::now();
while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) {
while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
std::this_thread::sleep_for(1ms);
}

EXPECT_TRUE(timer_completed);

// Shutdown needs to be called before join, so that executor.spin() returns.
rclcpp::shutdown();
// Cancel needs to be called before join, so that executor.spin() returns.
executor.cancel();
spinner.join();
executor.remove_node(this->node, true);
}

TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
Expand All @@ -195,16 +206,17 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
// Sleep for a short time to verify executor.spin() is going, and didn't throw.

auto start = std::chrono::steady_clock::now();
while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) {
while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
std::this_thread::sleep_for(1ms);
}

EXPECT_TRUE(timer_completed);
EXPECT_THROW(executor.spin(), std::runtime_error);

// Shutdown needs to be called before join, so that executor.spin() returns.
rclcpp::shutdown();
executor.cancel();
spinner.join();
executor.remove_node(this->node, true);
}

// Check executor exits immediately if future is complete.
Expand All @@ -223,7 +235,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
auto start = std::chrono::steady_clock::now();
auto shared_future = future.share();
auto ret = executor.spin_until_future_complete(shared_future, 1s);

executor.remove_node(this->node, true);
// Check it didn't reach timeout
EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start));
EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
Expand All @@ -245,6 +257,7 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) {
auto shared_future = future.share();
auto start = std::chrono::steady_clock::now();
auto ret = executor.spin_until_future_complete(shared_future, 1s);
executor.remove_node(this->node, true);

// Check it didn't reach timeout
EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start));
Expand Down Expand Up @@ -273,6 +286,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
std::thread spinner([&]() {
auto ret = executor.spin_until_future_complete(future, -1s);
EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
executor.remove_node(this->node, true);
executor.cancel();
spin_exited = true;
});

Expand All @@ -291,6 +306,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
// If this fails, the test will probably crash because spinner goes out of scope while the thread
// is active. However, it beats letting this run until the gtest timeout.
ASSERT_TRUE(spin_exited);
executor.cancel();
spinner.join();
}

Expand All @@ -316,6 +332,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) {
std::thread spinner([&]() {
auto ret = executor.spin_until_future_complete(future, 1ms);
EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret);
executor.remove_node(this->node, true);
spin_exited = true;
});

Expand Down Expand Up @@ -445,7 +462,7 @@ TYPED_TEST(TestExecutorsStable, spinAll) {
bool spin_exited = false;
std::thread spinner([&spin_exited, &executor, this]() {
executor.spin_all(1s);
executor.remove_node(this->node);
executor.remove_node(this->node, true);
spin_exited = true;
});

Expand Down Expand Up @@ -485,7 +502,7 @@ TYPED_TEST(TestExecutorsStable, spinSome) {
bool spin_exited = false;
std::thread spinner([&spin_exited, &executor, this]() {
executor.spin_some(1s);
executor.remove_node(this->node);
executor.remove_node(this->node, true);
spin_exited = true;
});

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -413,7 +413,10 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) {
TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) {
RclWaitSetSizes expected_sizes = {};
expected_sizes.size_of_subscriptions = 1;
if (std::string("rmw_connext_cpp") == rmw_get_implementation_identifier()) {
const std::string implementation_identifier = rmw_get_implementation_identifier();
if (implementation_identifier == "rmw_connext_cpp" ||
implementation_identifier == "rmw_cyclonedds_cpp")
{
// For connext, a subscription will also add an event and waitable
expected_sizes.size_of_events += 1;
expected_sizes.size_of_waitables += 1;
Expand Down