ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to know when spin() is up and running?

asked 2020-02-04 10:09:14 -0600

daixtrose gravatar image

Consider the following code example:

class Talker : public rclcpp::Node {
public:
    talker_component_EXPORT explicit Talker(const rclcpp::NodeOptions& options);
    void publish(std_msgs::msg::String s);

protected:
    // void on_timer();

private:
    size_t count_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

Talker::Talker(const rclcpp::NodeOptions& options)
    : Node("talker", options)
    , pub_{ create_publisher<std_msgs::msg::String>(
          "chatter",
          // Prefer reponsiveness over completeness of data
          rclcpp::QoS(rclcpp::KeepLast(1), rmw_qos_profile_sensor_data)) }
{
}

void Talker::publish(std_msgs::msg::String s) { pub_->publish(std::move(s)); }

class Listener : public rclcpp::Node {
public:
    listener_component_EXPORT explicit Listener(
        const rclcpp::NodeOptions& options,
        std::function<void(std_msgs::msg::String)> callback = [](auto s) {});

private:
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

Listener::Listener(
    const rclcpp::NodeOptions& options,
    std::function<void(std_msgs::msg::String)> callback)
    : Node("listener", options)
    , sub_{ create_subscription<std_msgs::msg::String>(
          "chatter",
          rclcpp::QoS{ rclcpp::KeepLast(1), rmw_qos_profile_sensor_data },
          [this, callback](const std::shared_ptr<std_msgs::msg::String> msg) {
              RCLCPP_INFO(
                  this->get_logger(), "I heard: [%s]", msg->data.c_str());
              callback(*msg);
          }) }
{
}


int main(int argc, char* argv[])
{
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);

    rclcpp::executors::MultiThreadedExecutor exec;
    rclcpp::NodeOptions options;

    auto talker = std::make_shared<composition::Talker>(options);
    exec.add_node(talker);

    using string_t = std_msgs::msg::String;
    std::vector<string_t> receivedMessages;

    auto reaction = [&receivedMessages](auto msg) {
        std::cerr << "Received " << msg.data;
        std::cerr << " in thread " << std::this_thread::get_id() << std::endl;
        receivedMessages.push_back(msg);
    };

    auto listener = std::make_shared<composition::Listener>(options, reaction);
    exec.add_node(listener);

    // Here we deviate from the toy example
    std::thread t([&exec]() {
        std::cerr << "Starting to spin" << std::endl;
        exec.spin();
    });

    using namespace std::chrono_literals;
    // MITIGATION
    // std::this_thread::sleep_for(1s);

    auto makeMsg = [](auto s) {
        string_t result;
        result.data = std::move(s);
        return result;
    };

    std::vector<string_t> sentMessages
        = { makeMsg("A"), makeMsg("B"), makeMsg("C") };

    auto task = [&sentMessages, &talker]() {
        for (auto const& msg : sentMessages) {
            std::cerr << "Publishing " << msg.data;
            std::cerr << " in thread " << std::this_thread::get_id()
                      << std::endl;
            talker->publish(msg);
            std::this_thread::sleep_for(1ms);
        }
    };

    std::thread t2(task);
    std::this_thread::sleep_for(2s);

    exec.cancel();
    t.join();
    t2.join();
    rclcpp::shutdown();

    std::cerr << "receivedMessages = ";
    for (auto msg : receivedMessages) {
        std::cerr << msg.data << ", ";
    }

    return 0;
}

Executor::spin() takes some time and therefore some messages might get lost, unless we introduce an explicit sleep command after calling spin.

Do you have another idea how to deal with this? I'd prefer an async_spin() giving me back a std::future on which I could wait. Any comments?

edit retag flag offensive close merge delete

Comments

Have you considered using a MultiThreadedSpinner instead of creating a new thread for spinning? Here is a code example https://yuzhangbit.github.io/tools/se...

lucascoelho gravatar image lucascoelho  ( 2020-02-04 11:12:22 -0600 )edit

I am not sure if your proposal fits into the ROS2 context.

daixtrose gravatar image daixtrose  ( 2020-02-04 15:57:54 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-02-04 12:27:58 -0600

tfoote gravatar image

Spin doesn't take significant time to start up what you're observing is the discovery and time required to establish connections. It's a race condition between creating your publisher, starting it publishing and making the connection to the subscriber. Spin doesn't necessarily have the state to know about whether there's a connection at the other end. And if you publish without any subscribers the message will be dropped.

If you want to have deterministic startup like this where you're coordinating the startup of publishers and subscribers to establish connections before starting to communicate please see the Lifecycle Nodes design and documentation

edit flag offensive delete link more

Comments

So IIUC you mean that if I am not using Lifecycle Nodes there is no possibility to obtain the information when all connections are established at runtime?

daixtrose gravatar image daixtrose  ( 2020-02-06 05:29:58 -0600 )edit

At a conceptual level, there's no definition of "all" to answer that question in a distributed system. If the publisher comes up first anything it publishes will have no subscribers until it heard the subscription request, then all goes from zero to one. And at a later time it could go up or down depending on if a new subscriber appears out there existing one disconnects.

Lifecycle let's you check that both sides of the pub sub link are initialized, so you know that there's not going to be a race condition at startup because both are already connected before you transition to active.

tfoote gravatar image tfoote  ( 2020-02-06 05:58:48 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2020-02-04 10:09:14 -0600

Seen: 1,046 times

Last updated: Feb 04 '20