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

Revision history [back]

click to hide/show revision 1
initial version

This has been a major pain point for me when porting a big ROS1 code base that made synchronous service calls all the time deep inside libraries. My preferred solution to this is setting up the node and client so that the service response arrives on another thread. Then the code calling the service can just do future.get() or future.wait() to wait for the response. You need to:

In general I try to do this and avoid use of spin_until_future_complete whereever possible because I don't want my internal application logic coupled with the spinning/execution of my node. Maybe we'll get some API updates in a future release that makes this easier, especially with having to handle callback group objects explicitly.

This has been a major pain point for me when porting a big ROS1 code base that made synchronous service calls all the time deep inside libraries. My preferred solution to this is setting up the node and client so that the service response arrives on another thread. Then the code calling the service can just do future.get() or future.wait() to wait for the response. You need to:

In general I try to do this and avoid use of spin_until_future_complete whereever possible because I don't want my internal application logic coupled with the spinning/execution of my node. Maybe we'll get some API updates in a future release that makes this easier, especially with having to handle callback group objects explicitly.

Edit: Including a full standalone example to clearly show how to do this. Here a node calls a service advertised by the same node from within the callback of a different service.

#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>

class TwoSrvs : public rclcpp::Node
{
public:
    TwoSrvs() : Node("two_srvs")
    {
      callback_group_ = this->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);

      outer_srv_ = this->create_service<std_srvs::srv::Trigger>("outer_srv",
          std::bind(&TwoSrvs::outer_cb, this, std::placeholders::_1, std::placeholders::_2),
          ::rmw_qos_profile_default,
          callback_group_);

      inner_srv_ = this->create_service<std_srvs::srv::Trigger>("inner_srv",
          std::bind(&TwoSrvs::inner_cb, this, std::placeholders::_1, std::placeholders::_2),
          ::rmw_qos_profile_default,
          callback_group_);

      client_ = this->create_client<std_srvs::srv::Trigger>("inner_srv");
  }

  void outer_cb(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr res)
  {
    using namespace std::chrono_literals;

    RCLCPP_INFO(get_logger(), "outer srv callback");

    auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
    auto future = client_->async_send_request(req);

    RCLCPP_INFO(get_logger(), "outer srv called inner srv");

    auto status = future.wait_for(3s);  //not spinning here!
    if (status == std::future_status::ready)
    {
      RCLCPP_INFO(get_logger(), "inner srv response is %s", future.get()->message.c_str());
      res->success = true;
      res->message = "good";
    }
    else
    {
      RCLCPP_ERROR(get_logger(), "inner srv future wait failed");
      res->success = false;
      res->message = "bad";
    }
  }

  void inner_cb(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr res)
  {
    RCLCPP_INFO(get_logger(), "inner srv callback");
    res->success = true;
    res->message = "good";
  }

  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr outer_srv_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr inner_srv_;
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_;
};

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

  auto node = std::make_shared<TwoSrvs>();

  rclcpp::executors::MultiThreadedExecutor exec;
  exec.add_node(node);
  exec.spin();
}