Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Some times, callback are not called during a call to Executor::spin_util_future_complete #717

Closed
adrienbarral opened this issue May 8, 2019 · 5 comments

Comments

@adrienbarral
Copy link

adrienbarral commented May 8, 2019

Bug report

Required Info:

  • Operating System:
    • Ubuntu 18.04
  • Installation type:
    • Binaries
  • Version or commit hash:
    • rclcpp version ros-crystal-rclcpp : 0.6.4-0bionic.20190406.051903
  • DDS implementation:
    • Fast-RTPS
  • Client library (if applicable):
    • rclcpp

Steps to reproduce issue

You can try this unit test file :

#include <chrono>
#include <gtest/gtest.h>
#include <rclcpp/executors.hpp>
#include <rclcpp/node.hpp>
#include <std_msgs/msg/float32.hpp>

using namespace std::literals;
using namespace rclcpp;
using namespace std_msgs::msg;
using namespace rclcpp::executor;

bool wait_for_subscriber(std::shared_ptr<rclcpp::Node> node,
                         const std::string& topic_name, std::chrono::milliseconds timeout,
                         std::chrono::microseconds sleep_period)
{
    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 = [&node, &topic_name]() -> bool {
        // the subscriber is available if the count is gt 0
        return node->count_subscribers(topic_name) > 0;
    };
    while(!predicate() && time_slept < duration_cast<microseconds>(timeout))
    {
        rclcpp::Event::SharedPtr graph_event = node->get_graph_event();
        node->wait_for_graph_change(graph_event, sleep_period);
        time_slept =
            duration_cast<std::chrono::microseconds>(steady_clock::now() - start);
    }
    int64_t time_slept_count =
        std::chrono::duration_cast<std::chrono::nanoseconds>(time_slept).count();
    RCLCPP_INFO(node->get_logger(),
                "Waited %d nanoseconds for the subscriber to connect to topic '%s'. Sub "
                "count : %d\n",
                time_slept_count, topic_name.c_str(),
                node->count_subscribers(topic_name));

    return !predicate();
}

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

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

class TesterNode : public Node
{
public:
    TesterNode() : Node("tester", "", true)
    {
        filteredSubscriber =
            create_subscription<Float32>("filtered", [this](Float32::SharedPtr in) {
                RCLCPP_INFO(get_logger(), "Filtered callback called");
                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>();

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

        ASSERT_FALSE(wait_for_subscriber(testerNode, "dataIn", 1000ms, 5ms));
    }

    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;
}

Expected behavior

I excepect that this test pass every time when I launch it.

Actual behavior

Some times, this test doesn't pass.
When this test doesn't pass it is always because the spin_util_future_complete return a timeout.

ASSERT_EQ(this->executor.spin_until_future_complete(future, std::chrono::seconds(1)), FutureReturnCode::SUCCESS);

When this occurs, I can't read neither the dataIn callback trace, nor the filtered trace. So that looks like the callback of "dataIn" subscription is never called.

Additional information

If I try to rise the timeout in spin_until_future_complete (to 10 seconds), test will also fail.

I also post this "bug" as question on Answers.ros.org because, I am not really sure that is a bug. Maybe I didn't well understood how executors work. But nobody answers there.

You can see that I try to use intraprocess comunication (I passed true as a third agument into my node's constructors). If let the default arguments, behaviour is the same.

@borgmanJeremy
Copy link

I looks like you only publish the message once? If so the message might be broadcast before the publisher has been been matched with a subscriber. You can use a service rather than pub/sub if you want to make sure an event driven message is delivered.

This thread may be relevant: ros2/ros2#344

@adrienbarral
Copy link
Author

@borgmanJeremy , I publish the message once, but before publishing it I wait for subscriber to be connected to publisher (thanks to the call to method wait_for_subscriber(testerNode, "dataIn", 1000ms, 5ms) ).

I think there is a big difference between my ticket and the one you linked. In my issue I am blocking execution of the thread until callback is called thanks to the spin_util_future_complete. In the issue you cite, there is a second thread in which spin is running.

@wjwwood
Copy link
Member

wjwwood commented May 10, 2019

I also post this "bug" as question on Answers.ros.org because, I am not really sure that is a bug. Maybe I didn't well understood how executors work. But nobody answers there.

Sorry, I've seen some of the questions about how the executor works and I've been wanting to relpy, but we've been super busy for the last few months. I had decided I would reply to those questions with links to updated documentation after doing some changes that were planned to the executor's architecture, but we ended up pushing that work off until later, so that also contributed to the delay.


I'll attempt to look into this issue in a few weeks, if no one else gets to it first.

@1r0b1n0
Copy link

1r0b1n0 commented May 20, 2019

Hi,
When running this code with __log_level:=debug I found some some interesting information :
It seems like sometimes subscriberNode->count_publishers(topic) returns "1" while the subscription isn't really finished yet.
Moreover there was a small error in the line looking for subscriber topic that has been fixed in ros2/rmw_fastrtps#279

Sometimes I have this output (fail) :
1558371495.682921408: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.682989051: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.688210422: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.688284384: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.693456493: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.693514447: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.693849354: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/rosout' with type 'rcl_interfaces::msg::dds_::Log_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.694061311: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/rosout' with type 'rcl_interfaces::msg::dds_::Log_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.694168301: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.694232581: [rcl] [DEBUG] - Waiting without timeout
1558371495.694236791: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parametersReply' with type 'rcl_interfaces::srv::dds_::GetParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.694264232: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.694334731: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.694390866: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.694553416: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/get_parametersReply' with type 'rcl_interfaces::srv::dds_::GetParameters_Response_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.694586377: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parameter_typesReply' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.694629206: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.694675479: [rcl] [DEBUG] - Waiting without timeout
1558371495.694697546: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.694766744: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.694821207: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.694919478: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parametersReply' with type 'rcl_interfaces::srv::dds_::SetParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.694981986: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/get_parameter_typesReply' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Response_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.695050443: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.695095095: [rcl] [DEBUG] - Waiting without timeout
1558371495.695118775: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.695188656: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.695252642: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parameters_atomicallyReply' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.695356968: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.695394768: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/set_parametersReply' with type 'rcl_interfaces::srv::dds_::SetParameters_Response_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.695461812: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.695506004: [rcl] [DEBUG] - Waiting without timeout
1558371495.695528658: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.695564931: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.695583987: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/describe_parametersReply' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.695671770: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.695814813: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/set_parameters_atomicallyReply' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Response_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.695882448: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.695927673: [rcl] [DEBUG] - Waiting without timeout
1558371495.695927669: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/list_parametersReply' with type 'rcl_interfaces::srv::dds_::ListParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.695953749: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.695982914: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.696046364: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.696237287: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/describe_parametersReply' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Response_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.696265737: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/parameter_events' with type 'rcl_interfaces::msg::dds_::ParameterEvent_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.696302205: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.696344017: [rcl] [DEBUG] - Waiting without timeout
1558371495.696367183: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.696420930: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.696475256: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.696646002: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/list_parametersReply' with type 'rcl_interfaces::srv::dds_::ListParameters_Response_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.696709920: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.696742945: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parametersRequest' with type 'rcl_interfaces::srv::dds_::GetParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.696758295: [rcl] [DEBUG] - Waiting without timeout
1558371495.696785236: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.696811666: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.696843283: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.697041684: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/parameter_events' with type 'rcl_interfaces::msg::dds_::ParameterEvent_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.697068140: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parameter_typesRequest' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.697109027: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.697151401: [rcl] [DEBUG] - Waiting without timeout
1558371495.697174847: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.697199548: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.697230991: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.697421645: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parametersRequest' with type 'rcl_interfaces::srv::dds_::SetParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.697689961: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/get_parametersRequest' with type 'rcl_interfaces::srv::dds_::GetParameters_Request_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.697762872: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.697790288: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parameters_atomicallyRequest' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.697809872: [rcl] [DEBUG] - Waiting without timeout
1558371495.697830520: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.697889383: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.697963907: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.698113457: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/get_parameter_typesRequest' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Request_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.698121263: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/list_parametersRequest' with type 'rcl_interfaces::srv::dds_::ListParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.698181444: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.698223443: [rcl] [DEBUG] - Waiting without timeout
1558371495.698247235: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.698283071: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.698327872: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.698519233: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/describe_parametersRequest' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.698530104: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/set_parametersRequest' with type 'rcl_interfaces::srv::dds_::SetParameters_Request_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.698599317: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.698645186: [rcl] [DEBUG] - Waiting without timeout
1558371495.698668262: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.698680909: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.698712246: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371495.698874369: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/dataIn' with type 'std_msgs::msg::dds_::Float32_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.698943439: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/set_parameters_atomicallyRequest' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Request_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.699007456: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.699051961: [rcl] [DEBUG] - Waiting without timeout
1558371495.699075165: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.699098660: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.699129454: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.699154303: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.699176292: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.699191683: [tester] [INFO] - Waited 105 ms for the subscriber to connect to topic 'dataIn'. Sub count : 1 Pub count : 1

1558371495.699270192: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.699294997: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371495.699316512: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parametersRequest' with type 'rcl_interfaces::srv::dds_::GetParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.699343628: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/list_parametersRequest' with type 'rcl_interfaces::srv::dds_::ListParameters_Request_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.699413272: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.699450927: [rcl] [DEBUG] - Waiting with timeout: 1s + 0ns
1558371495.699496932: [rcl] [DEBUG] - Waiting without timeout
1558371495.699532140: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.699555977: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.699624220: [rcl] [DEBUG] - Subscription in wait set is ready
1558371495.699653899: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.699671282: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.699684512: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.699740909: [rcl] [DEBUG] - Subscription taking message
1558371495.699760196: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/describe_parametersRequest' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Request_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.699742984: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parameter_typesRequest' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.699822098: [rcl] [DEBUG] - Subscription take succeeded: true
1558371495.699843470: [filter] [INFO] - Data In callback called
1558371495.699916114: [rcl] [DEBUG] - Waiting with timeout: 0s + 999477109ns
1558371495.699935302: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.699957842: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371495.699990633: [rcl] [DEBUG] - Waiting with timeout: 0s + 999393558ns
1558371495.700005611: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371495.700117870: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parametersRequest' with type 'rcl_interfaces::srv::dds_::SetParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.700179710: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/filtered' with type 'std_msgs::msg::dds_::Float32_' for node '1.f.74.1.17.5a.0.0.2.0.0.0|0.0.1.c1'
1558371495.700415935: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parameters_atomicallyRequest' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.700695843: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/describe_parametersRequest' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.700974107: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/list_parametersRequest' with type 'rcl_interfaces::srv::dds_::ListParameters_Request_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.701252756: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parametersReply' with type 'rcl_interfaces::srv::dds_::GetParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.701527716: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parameter_typesReply' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.701882581: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parametersReply' with type 'rcl_interfaces::srv::dds_::SetParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.702173134: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parameters_atomicallyReply' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.702449871: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/list_parametersReply' with type 'rcl_interfaces::srv::dds_::ListParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.702727570: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/describe_parametersReply' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Response_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.703116382: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/parameter_events' with type 'rcl_interfaces::msg::dds_::ParameterEvent_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
1558371495.703581579: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/filtered' with type 'std_msgs::msg::dds_::Float32_' for node '1.f.74.1.17.5a.0.0.1.0.0.0|0.0.1.c1'
/home/rov/Projects/udix/ws_dev/src/p_position_estimator/test/executor.test.cc:147: Failure
Expected equality of these values:
  this->executor.spin_until_future_complete(future, 1s)
    Which is: TIMEOUT (2)
  FutureReturnCode::SUCCESS
    Which is: SUCCESS (0)
And sometimes I have this output :
1558371875.306801711: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.312016097: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.312080992: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.317319466: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.317388988: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.317405914: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/rosout' with type 'rcl_interfaces::msg::dds_::Log_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.317494466: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.317560769: [rcl] [DEBUG] - Waiting without timeout
1558371875.317588318: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.317672680: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.317750285: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.317884273: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/get_parametersReply' with type 'rcl_interfaces::srv::dds_::GetParameters_Response_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.317947049: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.318000008: [rcl] [DEBUG] - Waiting without timeout
1558371875.318021757: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.318078823: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.318133430: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.318176759: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/get_parameter_typesReply' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Response_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.318231392: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.318270950: [rcl] [DEBUG] - Waiting without timeout
1558371875.318292093: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.318350756: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.318406671: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.318444857: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/set_parametersReply' with type 'rcl_interfaces::srv::dds_::SetParameters_Response_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.318496039: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.318536965: [rcl] [DEBUG] - Waiting without timeout
1558371875.318558532: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.318586781: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.318644341: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.318712680: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/set_parameters_atomicallyReply' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Response_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.318763962: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.318802139: [rcl] [DEBUG] - Waiting without timeout
1558371875.318823305: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.318882172: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.318936598: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.318994353: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/describe_parametersReply' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Response_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.319041974: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.319080196: [rcl] [DEBUG] - Waiting without timeout
1558371875.319101795: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.319160272: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.319215424: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.319363059: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/filter/list_parametersReply' with type 'rcl_interfaces::srv::dds_::ListParameters_Response_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.319466333: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.319538769: [rcl] [DEBUG] - Waiting without timeout
1558371875.319564394: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.319615522: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.319635728: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/parameter_events' with type 'rcl_interfaces::msg::dds_::ParameterEvent_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.319678719: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.319709770: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.319750440: [rcl] [DEBUG] - Waiting without timeout
1558371875.319772690: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.319830786: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.319886345: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.320047431: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/get_parametersRequest' with type 'rcl_interfaces::srv::dds_::GetParameters_Request_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.320101325: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.320142572: [rcl] [DEBUG] - Waiting without timeout
1558371875.320164194: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.320221698: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.320276448: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.320314649: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/get_parameter_typesRequest' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Request_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.320374998: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.320411590: [rcl] [DEBUG] - Waiting without timeout
1558371875.320432554: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.320468038: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.320535546: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.320593162: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/set_parametersRequest' with type 'rcl_interfaces::srv::dds_::SetParameters_Request_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.320638443: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.320677355: [rcl] [DEBUG] - Waiting without timeout
1558371875.320693706: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.320698556: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.320727155: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.320858224: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/set_parameters_atomicallyRequest' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Request_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.320906252: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.320944999: [rcl] [DEBUG] - Waiting without timeout
1558371875.320966870: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.321024843: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.321079593: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.321118707: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/list_parametersRequest' with type 'rcl_interfaces::srv::dds_::ListParameters_Request_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.321165364: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.321202146: [rcl] [DEBUG] - Waiting without timeout
1558371875.321223597: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.321281698: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.321337777: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.321377680: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/filter/describe_parametersRequest' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Request_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.321425365: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.321463520: [rcl] [DEBUG] - Waiting without timeout
1558371875.321484889: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.321543474: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.321598765: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.321712287: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/filtered' with type 'std_msgs::msg::dds_::Float32_' for node '1.f.74.1.b2.5a.0.0.2.0.0.0|0.0.1.c1'
1558371875.321763522: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.321804800: [rcl] [DEBUG] - Waiting without timeout
1558371875.321825814: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.321884986: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.321940460: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.327106955: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.327159275: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.332377082: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.332444746: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.337677733: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.337795385: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.342971608: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.343025791: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.348190415: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.348242149: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.353405040: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.353457957: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.358620881: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.358673410: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.363835272: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.363887679: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.369120913: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.369170057: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.374294510: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.374359172: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.379585288: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.379631574: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.384856516: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.384906092: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.387369958: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/rosout' with type 'rcl_interfaces::msg::dds_::Log_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.387657469: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parametersReply' with type 'rcl_interfaces::srv::dds_::GetParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.387885248: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parameter_typesReply' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.388085222: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parametersReply' with type 'rcl_interfaces::srv::dds_::SetParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.388278962: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parameters_atomicallyReply' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.388475220: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/describe_parametersReply' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.388677805: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/list_parametersReply' with type 'rcl_interfaces::srv::dds_::ListParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.388868451: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/parameter_events' with type 'rcl_interfaces::msg::dds_::ParameterEvent_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.389156803: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parametersRequest' with type 'rcl_interfaces::srv::dds_::GetParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.389357858: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parameter_typesRequest' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.389565063: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parametersRequest' with type 'rcl_interfaces::srv::dds_::SetParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.389817714: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parameters_atomicallyRequest' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.390014522: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/list_parametersRequest' with type 'rcl_interfaces::srv::dds_::ListParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.390021339: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.390066413: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 0
1558371875.390220207: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/describe_parametersRequest' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.390422378: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/dataIn' with type 'std_msgs::msg::dds_::Float32_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.390696782: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parametersRequest' with type 'rcl_interfaces::srv::dds_::GetParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.390914716: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/get_parameter_typesRequest' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.391122822: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parametersRequest' with type 'rcl_interfaces::srv::dds_::SetParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.391321630: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/set_parameters_atomicallyRequest' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.391526795: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/describe_parametersRequest' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.391723143: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rq/tester/list_parametersRequest' with type 'rcl_interfaces::srv::dds_::ListParameters_Request_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.391921853: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parametersReply' with type 'rcl_interfaces::srv::dds_::GetParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.392125794: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/get_parameter_typesReply' with type 'rcl_interfaces::srv::dds_::GetParameterTypes_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.392331699: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parametersReply' with type 'rcl_interfaces::srv::dds_::SetParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.392529384: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/set_parameters_atomicallyReply' with type 'rcl_interfaces::srv::dds_::SetParametersAtomically_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.392735854: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/list_parametersReply' with type 'rcl_interfaces::srv::dds_::ListParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.392979443: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rr/tester/describe_parametersReply' with type 'rcl_interfaces::srv::dds_::DescribeParameters_Response_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.393170538: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/parameter_events' with type 'rcl_interfaces::msg::dds_::ParameterEvent_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.393457207: [rmw_fastrtps_shared_cpp] [DEBUG] - Adding topic 'rt/filtered' with type 'std_msgs::msg::dds_::Float32_' for node '1.f.74.1.b2.5a.0.0.1.0.0.0|0.0.1.c1'
1558371875.395266047: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.395306885: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.395333530: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.395355217: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.395368535: [tester] [INFO] - Waited 177 ms for the subscriber to connect to topic 'dataIn'. Sub count : 1 Pub count : 1

1558371875.395426179: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.395444419: [rmw_fastrtps_shared_cpp] [DEBUG] - looking for subscriber topic: /dataIn, number of matches: 1
1558371875.395556211: [rcl] [DEBUG] - Waiting with timeout: 1s + 0ns
1558371875.395573728: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.395589660: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.395596895: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.395603217: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.395703392: [rcl] [DEBUG] - Waiting with timeout: 0s + 999813307ns
1558371875.395716401: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.395728220: [rcl] [DEBUG] - Subscription in wait set is ready
1558371875.395754355: [rcl] [DEBUG] - Subscription taking message
1558371875.395822416: [rcl] [DEBUG] - Subscription take succeeded: true
1558371875.395836469: [filter] [INFO] - Data In callback called
1558371875.395892339: [rcl] [DEBUG] - Waiting with timeout: 0s + 999623379ns
1558371875.395910334: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.395929061: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.395962716: [rcl] [DEBUG] - Waiting with timeout: 0s + 999551955ns
1558371875.395979741: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.396043356: [rcl] [DEBUG] - Subscription in wait set is ready
1558371875.396067530: [rcl] [DEBUG] - Subscription taking message
1558371875.396082254: [rcl] [DEBUG] - Subscription take succeeded: true
1558371875.396092227: [tester] [INFO] - Filtered callback called
1558371875.396151374: [rcl] [DEBUG] - Finalizing subscription
1558371875.396406141: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.396479443: [rcl] [DEBUG] - Waiting without timeout
1558371875.396493485: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.396530365: [rcl] [DEBUG] - Subscription finalized
1558371875.396552172: [rcl] [DEBUG] - Finalizing subscription
1558371875.396563719: [rcl] [DEBUG] - Subscription finalized
1558371875.396581824: [rcl] [DEBUG] - Finalizing publisher
1558371875.396593595: [rcl] [DEBUG] - Publisher finalized
1558371875.396606319: [rcl] [DEBUG] - Finalizing publisher
1558371875.396906123: [rcl] [DEBUG] - Publisher finalized
1558371875.396945973: [rcl] [DEBUG] - Finalizing client
1558371875.397330742: [rcl] [DEBUG] - Client finalized
1558371875.397356609: [rcl] [DEBUG] - Finalizing client
1558371875.397753448: [rcl] [DEBUG] - Client finalized
1558371875.397780135: [rcl] [DEBUG] - Finalizing client
1558371875.398316742: [rcl] [DEBUG] - Client finalized
1558371875.398353091: [rcl] [DEBUG] - Finalizing client
1558371875.398708969: [rcl] [DEBUG] - Client finalized
1558371875.398734025: [rcl] [DEBUG] - Finalizing client
1558371875.399056663: [rcl] [DEBUG] - Client finalized
1558371875.399080525: [rcl] [DEBUG] - Finalizing client
1558371875.399383252: [rcl] [DEBUG] - Client finalized
1558371875.399415030: [rcl] [DEBUG] - Finalizing subscription
1558371875.399534590: [rcl] [DEBUG] - Subscription finalized
1558371875.399553411: [rcl] [DEBUG] - Finalizing subscription
1558371875.399566818: [rcl] [DEBUG] - Subscription finalized
1558371875.399591073: [rcl] [DEBUG] - Finalizing service
1558371875.399868427: [rcl] [DEBUG] - Service finalized
1558371875.399892597: [rcl] [DEBUG] - Finalizing service
1558371875.400147906: [rcl] [DEBUG] - Service finalized
1558371875.400171994: [rcl] [DEBUG] - Finalizing service
1558371875.400418772: [rcl] [DEBUG] - Service finalized
1558371875.400442201: [rcl] [DEBUG] - Finalizing service
1558371875.400683640: [rcl] [DEBUG] - Service finalized
1558371875.400706211: [rcl] [DEBUG] - Finalizing service
1558371875.400935726: [rcl] [DEBUG] - Service finalized
1558371875.400958543: [rcl] [DEBUG] - Finalizing service
1558371875.401193138: [rcl] [DEBUG] - Service finalized
1558371875.401217335: [rcl] [DEBUG] - Finalizing publisher
1558371875.401232970: [rcl] [DEBUG] - Publisher finalized
1558371875.401246450: [rcl] [DEBUG] - Finalizing publisher
1558371875.401394493: [rcl] [DEBUG] - Publisher finalized
1558371875.401472880: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.401523172: [rcl] [DEBUG] - Waiting without timeout
1558371875.401535646: [rcl] [DEBUG] - Timeout calculated based on next scheduled timer: false
1558371875.401536634: [rcl] [DEBUG] - Finalizing node
1558371875.401571314: [rcl] [DEBUG] - Finalizing publisher
1558371875.401759330: [rcl] [DEBUG] - Publisher finalized
1558371875.403054009: [rcl] [DEBUG] - Node finalized
1558371875.403093511: [rcl] [DEBUG] - Finalizing subscription
1558371875.403202579: [rcl] [DEBUG] - Subscription finalized
1558371875.403219583: [rcl] [DEBUG] - Finalizing subscription
1558371875.403231992: [rcl] [DEBUG] - Subscription finalized
1558371875.403248743: [rcl] [DEBUG] - Finalizing publisher
1558371875.403260442: [rcl] [DEBUG] - Publisher finalized
1558371875.403272907: [rcl] [DEBUG] - Finalizing publisher
1558371875.403473081: [rcl] [DEBUG] - Publisher finalized
1558371875.403495012: [rcl] [DEBUG] - Finalizing client
1558371875.403787255: [rcl] [DEBUG] - Client finalized
1558371875.403807353: [rcl] [DEBUG] - Finalizing client
1558371875.404092938: [rcl] [DEBUG] - Client finalized
1558371875.404111970: [rcl] [DEBUG] - Finalizing client
1558371875.404365937: [rcl] [DEBUG] - Client finalized
1558371875.404384880: [rcl] [DEBUG] - Finalizing client
1558371875.404645655: [rcl] [DEBUG] - Client finalized
1558371875.404664067: [rcl] [DEBUG] - Finalizing client
1558371875.404909799: [rcl] [DEBUG] - Client finalized
1558371875.404929267: [rcl] [DEBUG] - Finalizing client
1558371875.405149546: [rcl] [DEBUG] - Client finalized
1558371875.405175218: [rcl] [DEBUG] - Finalizing subscription
1558371875.405252688: [rcl] [DEBUG] - Subscription finalized
1558371875.405269047: [rcl] [DEBUG] - Finalizing subscription
1558371875.405282618: [rcl] [DEBUG] - Subscription finalized
1558371875.405297957: [rcl] [DEBUG] - Finalizing service
1558371875.405499180: [rcl] [DEBUG] - Service finalized
1558371875.405518555: [rcl] [DEBUG] - Finalizing service
1558371875.405796585: [rcl] [DEBUG] - Service finalized
1558371875.405819377: [rcl] [DEBUG] - Finalizing service
1558371875.406008170: [rcl] [DEBUG] - Service finalized
1558371875.406027360: [rcl] [DEBUG] - Finalizing service
1558371875.406208344: [rcl] [DEBUG] - Service finalized
1558371875.406227100: [rcl] [DEBUG] - Finalizing service
1558371875.406400080: [rcl] [DEBUG] - Service finalized
1558371875.406418929: [rcl] [DEBUG] - Finalizing service
1558371875.406593349: [rcl] [DEBUG] - Service finalized
1558371875.406612676: [rcl] [DEBUG] - Finalizing publisher
1558371875.406626641: [rcl] [DEBUG] - Publisher finalized
1558371875.406638738: [rcl] [DEBUG] - Finalizing publisher
1558371875.406747473: [rcl] [DEBUG] - Publisher finalized
1558371875.406780907: [rcl] [DEBUG] - Finalizing node
1558371875.406795656: [rcl] [DEBUG] - Finalizing publisher
1558371875.406881808: [rcl] [DEBUG] - Publisher finalized
1558371875.407804589: [rcl] [DEBUG] - Node finalized
[       OK ] TestFixture.weCanFilterOurDatas (219 ms)
[----------] 1 test from TestFixture (219 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test case ran. (219 ms total)
[  PASSED  ] 1 test.
1558371875.407956914: [rcl] [DEBUG] - Shutting down ROS client library, for context at address: 0x5624767af070
1558371875.408034781: [rcl] [DEBUG] - Guard condition in wait set is ready
1558371875.408115849: [rclcpp] [DEBUG] - SignalHandler::uninstall(): notifying deferred signal handler
1558371875.408147437: [rclcpp] [DEBUG] - deferred_signal_handler(): woken up due to SIGINT or uninstall
1558371875.408177989: [rclcpp] [DEBUG] - deferred_signal_handler(): signal handling uninstalled
1558371875.408236462: [rclcpp] [DEBUG] - signal handler uninstalled

@adrienbarral
Copy link
Author

Ok, I think I found what is the problem, and of course, the problem is not in rclcpp, but in my test...

When reading carefully logs in the @1r0b1n0 message, I can see that when test fail, this is because topic filtered is added after we finished waiting for topic dataIn.
And when the test pass, filtered is added before dataIn.

So the problem is just that we must wait for both connections to be established before continuing !!!

Adding the following line in my SetUp function make the test pass every times :

 ASSERT_FALSE(wait_for_subscriber(testerNode, "dataIn", 1000ms, 5ms));
 ASSERT_FALSE(wait_for_subscriber(filterNode, "filtered", 1000ms, 5ms));

nnmm pushed a commit to ApexAI/rclcpp that referenced this issue Jul 9, 2022
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

4 participants