// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// 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.
#include
#include
#include
#include
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rclcpp/rclcpp.hpp"
#include "test_msgs/msg/basic_types.hpp"
#include "../utils/rclcpp_gtest_macros.hpp"
class TestWaitSet : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
};
/*
* Testing normal construction and destruction.
*/
TEST_F(TestWaitSet, construction_and_destruction) {
{
rclcpp::WaitSet wait_set;
(void)wait_set;
}
{
rclcpp::WaitSet wait_set(
std::vector<:waitset::subscriptionentry>{},
std::vector<:guardcondition::sharedptr>{},
std::vector<:timerbase::sharedptr>{},
std::vector<:clientbase::sharedptr>{},
std::vector<:servicebase::sharedptr>{},
std::vector<:waitset::waitableentry>{});
(void)wait_set;
}
{
auto gc = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set({}, {gc});
(void)wait_set;
}
{
auto context = std::make_shared<:context>();
context->init(0, nullptr);
auto gc = std::make_shared<:guardcondition>(context);
rclcpp::WaitSet wait_set({}, {gc}, {}, {}, {}, {}, context);
(void)wait_set;
}
{
// invalid context (nullptr)
ASSERT_THROW(
{
rclcpp::WaitSet wait_set(
std::vector<:waitset::subscriptionentry>{},
std::vector<:guardcondition::sharedptr>{},
std::vector<:timerbase::sharedptr>{},
std::vector<:clientbase::sharedptr>{},
std::vector<:servicebase::sharedptr>{},
std::vector<:waitset::waitableentry>{},
nullptr);
(void)wait_set;
}, std::invalid_argument);
}
{
// invalid context (uninitialized)
auto context = std::make_shared<:context>();
ASSERT_THROW(
{
rclcpp::WaitSet wait_set(
std::vector<:waitset::subscriptionentry>{},
std::vector<:guardcondition::sharedptr>{},
std::vector<:timerbase::sharedptr>{},
std::vector<:clientbase::sharedptr>{},
std::vector<:servicebase::sharedptr>{},
std::vector<:waitset::waitableentry>{},
context);
(void)wait_set;
}, rclcpp::exceptions::RCLInvalidArgument);
}
}
/*
* Testing rcl wait set accessor.
*/
TEST_F(TestWaitSet, get_rcl_wait_set) {
{
rclcpp::WaitSet wait_set;
wait_set.get_rcl_wait_set();
}
}
/*
* Testing add/remove for guard condition methods.
*/
TEST_F(TestWaitSet, add_remove_guard_condition) {
// normal, mixed initialization
{
auto gc = std::make_shared<:guardcondition>();
auto gc2 = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set({}, {gc});
wait_set.add_guard_condition(gc2);
wait_set.remove_guard_condition(gc2);
wait_set.remove_guard_condition(gc);
}
// out of order removal
{
auto gc = std::make_shared<:guardcondition>();
auto gc2 = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set({}, {gc});
wait_set.add_guard_condition(gc2);
wait_set.remove_guard_condition(gc);
wait_set.remove_guard_condition(gc2);
}
// start empty, normal
{
auto gc = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set;
wait_set.add_guard_condition(gc);
wait_set.remove_guard_condition(gc);
}
// add invalid (nullptr)
{
rclcpp::WaitSet wait_set;
ASSERT_THROW(
{
wait_set.add_guard_condition(nullptr);
}, std::invalid_argument);
}
// double add
{
auto gc = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set;
wait_set.add_guard_condition(gc);
ASSERT_THROW(
{
wait_set.add_guard_condition(gc);
}, std::runtime_error);
}
// remove invalid (nullptr)
{
rclcpp::WaitSet wait_set;
ASSERT_THROW(
{
wait_set.remove_guard_condition(nullptr);
}, std::invalid_argument);
}
// remove unrelated
{
auto gc = std::make_shared<:guardcondition>();
auto gc2 = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set({}, {gc});
ASSERT_THROW(
{
wait_set.remove_guard_condition(gc2);
}, std::runtime_error);
}
// double remove
{
auto gc = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set({}, {gc});
wait_set.remove_guard_condition(gc);
ASSERT_THROW(
{
wait_set.remove_guard_condition(gc);
}, std::runtime_error);
}
// remove from empty
{
auto gc = std::make_shared<:guardcondition>();
rclcpp::WaitSet wait_set;
ASSERT_THROW(
{
wait_set.remove_guard_condition(gc);
}, std::runtime_error);
}
}
/*
* Testing adding each entity to two separate wait sets.
*/
TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) {
{
rclcpp::WaitSet wait_set1;
rclcpp::WaitSet wait_set2;
auto node = std::make_shared<:node>("add_guard_condition_to_two_different_wait_set");
auto guard_condition = std::make_shared<:guardcondition>();
wait_set1.add_guard_condition(guard_condition);
ASSERT_THROW(
{
wait_set2.add_guard_condition(guard_condition);
}, std::runtime_error);
auto do_nothing = [](std::shared_ptr) {};
auto sub = node->create_subscription<:msg::basictypes>("~/test", 1, do_nothing);
wait_set1.add_subscription(sub);
ASSERT_THROW(
{
wait_set2.add_subscription(sub);
}, std::runtime_error);
auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {});
wait_set1.add_timer(timer);
ASSERT_THROW(
{
wait_set2.add_timer(timer);
}, std::runtime_error);
auto client = node->create_client<:srv::listparameters>("~/test");
wait_set1.add_client(client);
ASSERT_THROW(
{
wait_set2.add_client(client);
}, std::runtime_error);
auto srv_do_nothing = [](
const std::shared_ptr<:srv::listparameters::request>,
std::shared_ptr<:srv::listparameters::response>) {};
auto service =
node->create_service<:srv::listparameters>("~/test", srv_do_nothing);
wait_set1.add_service(service);
ASSERT_THROW(
{
wait_set2.add_service(service);
}, std::runtime_error);
rclcpp::PublisherOptions po;
po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {};
auto pub = node->create_publisher<:msg::basictypes>("~/test", 1, po);
auto qos_event = pub->get_event_handlers()[0];
wait_set1.add_waitable(qos_event, pub);
ASSERT_THROW(
{
wait_set2.add_waitable(qos_event, pub);
}, std::runtime_error);
}
}
/*
* Testing adding each entity and waiting, and removing each entity and waiting
*/
TEST_F(TestWaitSet, add_remove_wait) {
rclcpp::WaitSet wait_set;
auto node = std::make_shared<:node>("add_remove_wait");
auto guard_condition = std::make_shared<:guardcondition>();
guard_condition->trigger();
// For coverage reasons, this subscription should have event handlers
rclcpp::SubscriptionOptions subscription_options;
subscription_options.event_callbacks.deadline_callback = [](auto) {};
subscription_options.event_callbacks.liveliness_callback = [](auto) {};
auto do_nothing = [](std::shared_ptr) {};
auto sub =
node->create_subscription<:msg::basictypes>(
"~/test", 1, do_nothing, subscription_options);
auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
auto client = node->create_client<:srv::listparameters>("~/test");
auto srv_do_nothing = [](
const std::shared_ptr<:srv::listparameters::request>,
std::shared_ptr<:srv::listparameters::response>) {};
auto service =
node->create_service<:srv::listparameters>("~/test", srv_do_nothing);
rclcpp::PublisherOptions publisher_options;
publisher_options.event_callbacks.deadline_callback =
[](rclcpp::QOSDeadlineOfferedInfo &) {};
auto pub = node->create_publisher<:msg::basictypes>(
"~/test", 1, publisher_options);
auto qos_event = pub->get_event_handlers()[0];
// Subscription mask is required here for coverage.
wait_set.add_subscription(sub, {true, true, true});
wait_set.add_guard_condition(guard_condition);
wait_set.add_timer(timer);
wait_set.add_client(client);
wait_set.add_service(service);
wait_set.add_waitable(qos_event, pub);
// At least timer or guard_condition should trigger
EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind());
wait_set.remove_subscription(sub, {true, true, true});
wait_set.remove_guard_condition(guard_condition);
wait_set.remove_timer(timer);
wait_set.remove_client(client);
wait_set.remove_service(service);
wait_set.remove_waitable(qos_event);
EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait(std::chrono::seconds(1)).kind());
}
/*
* Get wait_set from result.
*/
TEST_F(TestWaitSet, get_result_from_wait_result) {
rclcpp::WaitSet wait_set;
auto guard_condition = std::make_shared<:guardcondition>();
wait_set.add_guard_condition(guard_condition);
guard_condition->trigger();
rclcpp::WaitResult<:waitset> result = wait_set.wait();
ASSERT_EQ(rclcpp::WaitResultKind::Ready, result.kind());
EXPECT_EQ(&wait_set, &result.get_wait_set());
const rclcpp::WaitResult<:waitset> const_result(std::move(result));
ASSERT_EQ(rclcpp::WaitResultKind::Ready, const_result.kind());
EXPECT_EQ(&wait_set, &const_result.get_wait_set());
}
TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
rclcpp::WaitSet wait_set;
auto guard_condition = std::make_shared<:guardcondition>();
wait_set.add_guard_condition(guard_condition);
rclcpp::WaitResult<:waitset> result = wait_set.wait(std::chrono::milliseconds(10));
ASSERT_EQ(rclcpp::WaitResultKind::Timeout, result.kind());
RCLCPP_EXPECT_THROW_EQ(
result.get_wait_set(),
std::runtime_error("cannot access wait set when the result was not ready"));
const rclcpp::WaitResult<:waitset> const_result(std::move(result));
ASSERT_EQ(rclcpp::WaitResultKind::Timeout, const_result.kind());
RCLCPP_EXPECT_THROW_EQ(
const_result.get_wait_set(),
std::runtime_error("cannot access wait set when the result was not ready"));
}