0
$\begingroup$

Rosanswers logo

Hello,

Assume I have a "filter node" that subscribe to a data, transform it, and re-publish it.

class Filter : public Node
{
public :
    Filter() : Node("filter", "", true)
    {
        filteredDataPub = create_publisher<Float32>("filtered");
        dataIn = create_subscription<Float32>("dataIn", [this](Float32::SharedPtr in) {
            auto res = std::make_shared<Float32>();
            res->data = in->data * 2;
            filteredDataPub->publish(res);
        });
    }

protected:
    Publisher<Float32>::SharedPtr filteredDataPub;
    Subscription<Float32>::SharedPtr dataIn;
};

I would like to test it.

So I Wrote the following test :

class TesterNode : public Node
{
public:
    TesterNode() : Node("tester", "", true)
    {
        filteredSubscriber =
            create_subscription<Float32>("filtered", [this](Float32::SharedPtr in) {
                filteredPromise.set_value(in->data);
            });
        dataInPublisher = create_publisher<Float32>("dataIn");
    }
    void publishData(float data)
    {
        auto res = std::make_shared<Float32>();
        res->data = data;
        dataInPublisher->publish(res);
    }
    std::promise<float> filteredPromise;

protected:
    rclcpp::Publisher<Float32>::SharedPtr dataInPublisher;
    rclcpp::Subscription<Float32>::SharedPtr filteredSubscriber;
};

class TestFixture : public ::testing::Test
{
protected:
    void SetUp()
    {

        testerNode = std::make_shared<TesterNode>();
        ASSERT_EQ(testerNode->count_subscribers("dataIn"), static_cast<size_t>(0));
        filterNode = std::make_shared<Filter>();
        ASSERT_EQ(filterNode->count_publishers("dataIn"), static_cast<size_t>(0));

        executor.add_node(filterNode);
        executor.add_node(testerNode);

        ASSERT_FALSE(rclpp_utils::wait_for_subscriber_and_publisher(testerNode, filterNode, "dataIn", 1000ms));
    }

    void TearDown()
    {
        executor.cancel();
        executor.remove_node(filterNode);
        executor.remove_node(testerNode);
        testerNode.reset();
        filterNode.reset();
    }

    std::shared_ptr<TesterNode> testerNode;
    rclcpp::executors::SingleThreadedExecutor executor;

private:
    std::shared_ptr<Filter> filterNode;
};

TEST_F(TestFixture, weCanFilterOurDatas)
{
    auto future = std::shared_future<float>(testerNode->filteredPromise.get_future());
    testerNode->publishData(5.0);
    ASSERT_EQ(this->executor.spin_until_future_complete(future, std::chrono::seconds(1)),
              FutureReturnCode::SUCCESS);
    ASSERT_EQ(future.get(), 10.);
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    ::testing::InitGoogleTest(&argc, argv);
    auto res = RUN_ALL_TESTS();
    rclcpp::shutdown();
    return res;
}

As you can see "TesterNode" is here to "trigger" the Filter Node, and TestFixture is the boilerplate required to instanciate both nodes.

"wait_for_subscriber_and_publisher" function is a patched version of the one we can found in system_tests. In my version the method return true if there is a timeout, and argument "to be available" is forced to TRUE. And in my version I wait for both publisher and subscriber.

bool wait_for_subscriber_and_publisher(std::shared_ptr<rclcpp::Node> publisherNode,
                    std::shared_ptr<rclcpp::Node> subscriberNode,
                    const std::string& topic_name, std::chrono::milliseconds timeout)
{
    using std::chrono::duration_cast;
    using std::chrono::microseconds;
    using std::chrono::steady_clock;
    auto start = steady_clock::now();
    microseconds time_slept(0);
    auto predicate = [&publisherNode, &subscriberNode, &topic_name]() -> bool {
        // the subscriber is available if the count is gt 0
        return publisherNode->count_subscribers(topic_name) > 0 &&
               subscriberNode->count_publishers(topic_name) > 0;
    };
    while(!predicate() && time_slept < duration_cast<microseconds>(timeout))
    {
        rclcpp::Event::SharedPtr graph_event = publisherNode->get_graph_event();
        publisherNode->wait_for_graph_change(graph_event, 5ms);
        time_slept =
            duration_cast<std::chrono::microseconds>(steady_clock::now() - start);
    }
    int64_t time_slept_count =
        std::chrono::duration_cast<std::chrono::milliseconds>(time_slept).count();
    RCLCPP_INFO(publisherNode->get_logger(),
                "Waited %d ms for the subscriber to connect to topic '%s'. Sub "
                "count : %d Pub count : %d\n",
                time_slept_count, topic_name.c_str(),
                publisherNode->count_subscribers(topic_name),
                subscriberNode->count_publishers(topic_name));

    return !predicate();
}

When I run my test several times, some times test pass, some times test doesn't pass. When test fails, this is because spin_until_future_complete return a TIMEOUT. As you can see I force my nodes to use intra process comunication (passing true as third argument of node constructor). But even if I use the default arguments, I have my problem.

I carrefully read unit tests of rclcpp and system_tests (but maybe not enough carrefully...). I focus my attention on test_subscription.cpp, and I can't really see difference with my test. I guess test_subscription work every time, isn't it ?

So my first question is : Does the above example is supposed to work ? Or what's wrong with it ?


Originally posted by Adrien BARRAL on ROS Answers with karma: 11 on 2019-05-07

Post score: 1

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

I was able to reproduce the issue by inducing stress on my machine (e.g. stress --cpu 8). I tried also waiting for the publisher and that seemed to resolve the issue. I guess it's possible that even though the publisher can see the subscriber, the inverse is not necessarily true. Therefore, ensuring that both can see each other before executing the test will hopefully fix the problem.


Originally posted by jacobperron with karma: 1870 on 2019-05-10

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by Adrien BARRAL on 2019-05-11:
I try to wait for publisher and subscriber as you suggest, but the problem is still there... I will update the question to take your remark into account.

Comment by jacobperron on 2019-05-13:
Could you also include the implementation of rclpp_utils::wait_for_subscriber_and_publisher?

Comment by Adrien BARRAL on 2019-05-13:
I updated the question.

$\endgroup$
0
$\begingroup$

Rosanswers logo

One of the things I was experimenting with was making the nodes lifecycle nodes. It's not ideal because you are now forced to make things lifecycle enabled solely for the purpose of testing. However, I think the amount of extra code can be minimal.

You can see an example here: https://github.com/ros-planning/navigation2/blob/master/nav2_util/test/test_lifecycle_utils.cpp

This test just brings up two lifecycle nodes and ensures they are activated. However, you could extend from there to do your specific test after that point.


Originally posted by Carl D with karma: 303 on 2019-05-10

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by tfoote on 2019-05-10:
This is the type of determinism that lifecycle nodes are designed to support.

Comment by Adrien BARRAL on 2019-05-11:
Does that means that "raw nodes" are not designed to support this type of determinism ?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.