Robotics StackExchange | Archived questions

Service callback blocks subscriber callback with MultiThreadedExecutor

Steps to reproduce issue

  1. Create a node with a subscriber and a service.
  2. Either use two MutuallyExclusive or one Reentrant callback group.
  3. Spin the node in a MultiThreadedExecutor

```

include

include

include

include

include "rclcpp/rclcpp.hpp"

include "std_msgs/msg/string.hpp"

include "std_srvs/srv/empty.hpp"

using namespace std::placeholders;

class MultiThreadedNode : public rclcpp::Node { public: MultiThreadedNode() : Node("multithreadednode") { auto defaultqos = rclcpp::QoS(10); cbgroup1 = createcallbackgroup(rclcpp::CallbackGroupType::MutuallyExclusive); cbgroup2_ = createcallbackgroup(rclcpp::CallbackGroupType::MutuallyExclusive); cbgroup3_ = createcallbackgroup(rclcpp::CallbackGroupType::MutuallyExclusive);

    rclcpp::SubscriptionOptions sub_options;
    sub_options.callback_group = cb_group_1_;

    subscriber_ = create_subscription<std_msgs::msg::String>(
        "chatter",
        default_qos, 
        std::bind(&MultiThreadedNode::subscriber_callback, this, _1),
        sub_options);

    service_ = create_service<std_srvs::srv::Empty>(
        "service",
        std::bind(&MultiThreadedNode::service_callback, this, _1, _2),
        rmw_qos_profile_default,
        cb_group_2_);

    timer_ = create_wall_timer(
        std::chrono::seconds(2),
        std::bind(&MultiThreadedNode::timer_callback, this),
        cb_group_3_);
}

private: void subscribercallback(const stdmsgs::msg::String::SharedPtr msg) { (void)msg; RCLCPPINFO(getlogger(), "."); }

void service_callback(
    const std_srvs::srv::Empty::Request::SharedPtr req,
    const std_srvs::srv::Empty::Response::SharedPtr res)
{
    (void)req;
    (void)res;
    RCLCPP_INFO(this->get_logger(), "enter service callback");
    for (size_t i = 0; i < 10; i++)
    {
        RCLCPP_INFO(this->get_logger(), "#");
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    RCLCPP_INFO(this->get_logger(), "exit service callback");
}

void timer_callback()
{
    RCLCPP_INFO(this->get_logger(), "enter timer callback");
    for (int i = 0; i < 10; ++i) {
        RCLCPP_INFO(this->get_logger(), "+");
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    RCLCPP_INFO(this->get_logger(), "exit timer callback");
}

rclcpp::CallbackGroup::SharedPtr cb_group_1_;
rclcpp::CallbackGroup::SharedPtr cb_group_2_;
rclcpp::CallbackGroup::SharedPtr cb_group_3_;

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_;
rclcpp::TimerBase::SharedPtr timer_;

};

int main(int argc, char * argv[]) { rclcpp::init(argc, argv);

auto node = std::make_shared<MultiThreadedNode>();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();

rclcpp::shutdown();
return 0;

} ```

Expected behavior

Calling the service should not block the subscriber or the timer.

Actual behavior

Calling the service blocks the subscriber but not the timer.

```

[INFO] [1680471757.115031455] [multithreadednode]: . [INFO] [1680471757.314653746] [multithreadednode]: . [INFO] [1680471757.514849401] [multithreadednode]: . [INFO] [1680471757.714504254] [multithreadednode]: . [INFO] [1680471757.807676140] [multithreadednode]: enter timer callback [INFO] [1680471757.807809920] [multithreadednode]: + [INFO] [1680471757.908003562] [multithreadednode]: + [INFO] [1680471757.915152829] [multithreadednode]: . [INFO] [1680471758.008272385] [multithreadednode]: + [INFO] [1680471758.108555032] [multithreadednode]: + [INFO] [1680471758.114981067] [multithreadednode]: . [INFO] [1680471758.208821078] [multithreadednode]: + [INFO] [1680471758.296828261] [multithreadednode]: enter service callback [INFO] [1680471758.296945886] [multithreadednode]: # [INFO] [1680471758.309048233] [multithreadednode]: + [INFO] [1680471758.397140516] [multithreadednode]: # [INFO] [1680471758.409268071] [multithreadednode]: + [INFO] [1680471758.497408827] [multithreadednode]: # [INFO] [1680471758.509478168] [multithreadednode]: + [INFO] [1680471758.597679190] [multithreadednode]: # [INFO] [1680471758.609651464] [multithreadednode]: + [INFO] [1680471758.697816595] [multithreadednode]: # [INFO] [1680471758.709744064] [multithreadednode]: + [INFO] [1680471758.797935812] [multithreadednode]: # [INFO] [1680471758.809839482] [multithreadednode]: exit timer callback [INFO] [1680471758.898066940] [multithreadednode]: # [INFO] [1680471758.998168237] [multithreadednode]: # [INFO] [1680471759.098295381] [multithreadednode]: # [INFO] [1680471759.198418182] [multithreadednode]: # [INFO] [1680471759.298543627] [multithreadednode]: exit service callback [INFO] [1680471759.305546736] [multithreadednode]: . [INFO] [1680471759.514772973] [multithreadednode]: . [INFO] [1680471759.714968133] [multithreadednode]: . [INFO] [1680471759.807685939] [multithreadednode]: enter timer callback [INFO] [1680471759.807810022] [multithreadednode]: + [INFO] [1680471759.908048993] [multithreadednode]: + [INFO] [1680471759.914723904] [multithreadednode]: . [INFO] [1680471760.008308786] [multithreadednode]: + [INFO] [1680471760.108570934] [multithreadednode]: + [INFO] [1680471760.114304442] [multithreadednode]: . [INFO] [1680471760.208807294] [multithreadednode]: + [INFO] [1680471760.309081122] [multithreadednode]: + [INFO] [1680471760.314686328] [multithreadednode]: . [INFO] [1680471760.409337562] [multithreadednode]: + [INFO] [1680471760.509623742] [multithreadednode]: + [INFO] [1680471760.514945516] [multithreadednode]: . [INFO] [1680471760.609898005] [multithreadednode]: + [INFO] [1680471760.710182078] [multithreadednode]: + [INFO] [1680471760.714599593] [multithreadednode]: . [INFO] [1680471760.810430563] [multithreadednode]: exit timer callback [INFO] [1680471760.914164703] [multithreadednode]: . [INFO] [1680471761.114127971] [multithreadednode]: . [INFO] [1680471761.314642288] [multithreadednode]: . [INFO] [1680471761.514949373] [multithreadednode]: . [INFO] [1680471761.714710297] [multithreadednode]: .

```

Additional information

Please don't tell me to use an action. :)

Asked by lem2300 on 2023-04-02 17:14:35 UTC

Comments

Answers