Using coroutines with service calls

• Repo Link: https://github.com/ctu-mrs/mrs_lib/tree/ros2/examples/coro_service

• Expected prerequisites:

  • ROS2 Packages

  • ROS2 Nodes

  • ROS2 Services

Coroutines allow suspending functions and resuming them later. In context of ROS2 programs, this is very useful when calling services. If you do not use coroutines, the options used are usually either blocking on the future (cannot be done in single threaded context) or manually checking it (requires timers for checking readiness) or specifying callbacks. Coroutines solve this by writing code, that looks almost like the blocking case, while being functionally similar to writing the rest of the function as a callback.

Note

This tutorial covers how to use coroutines specifically with service client in mrs_lib. If you want more general information and how they work, see eg. https://cppreference.com/cpp/language/coroutines.

Important parts

In this section, we will walk through the important parts of the example. The full source code can be found at the bottom of this page or in the examples folder of mrs_lib repo.

Coroutines

Syntactically, coroutines have two major differences normal C++ functions.

1. Firstly, they must contain at least one of the co_* keywords (co_await, co_return or co_yield).

2. Secondly, the return type must be a coroutine return type. For our usecase, it is enough to know that mrs_lib::Task<T> is such a type. The template parameter T is the type returned from the coroutine when awaited (defaults to void).

An example coroutine is shown in the following listing. The coroutine returns std::optional<std::shared_ptr<ServiceType::Response>> which is wrapped in mrs_lib::Task. It takes parameters just like a normal function.

Instead of the normal return keyword, we must use co_return (just coroutine version of the same). The co_await keyword in front of the client call tells the language to suspend the current coroutine here and resume once the awaited result is ready. In this case, it means it sends the service request, suspends the coroutine, and resumes it once the server’s response arives.

Example coroutine.

mrs_lib::Task<std::optional<std::shared_ptr<ServiceType::Response>>> call_service(bool data)
{
  auto request = std::make_shared<ServiceType::Request>();
  request->data = data;
  co_return co_await service_client_.callAwaitable(request);
}

As you can see, the coroutine function looks almost like a normal function, just with some co_* keywords sprinkled in.

Next we will have a look at a second coroutine in our program. This time, it will be a timer callback.

Example coroutine callback.

mrs_lib::Task<> service_timer_callback()
{
  RCLCPP_INFO_STREAM(logger, "Calling service...");
  std::optional<std::shared_ptr<ServiceType::Response>> response_opt = co_await call_service(true);
  if (response_opt.has_value())
  {
    auto response = response_opt.value();
    std::string response_str = std::format("success: '{}'\n  message: '{}'", response->success, response->message);
    RCLCPP_INFO_STREAM(logger, "Service response:\n" << response_str);
  } else
  {
    RCLCPP_WARN_STREAM(logger, "Failed to call service!!!");
  }
}

Once again, you can see it uses the co_await keyword. This is required whenever you are calling another coroutine, even if it returns void.

You may also notice that there is no co_return this time. Similarly to normal functions, if the coroutine returns void, you can omit the co_return at the end. However, you still have to have at least one co_* keyword in it to make it a coroutine.

Warning

The coroutines implemented in mrs_lib are so called lazy coroutines. This means they do nothing until you await them (eg. co_await my_coro()). As a result, if you forget to await a coroutine, it will not run!

Coroutines as callbacks

You can use coroutines as callbacks, but there are currently some additional restrictions:

1. The object which calling the callbacks must support calling coroutines. (See API reference for the individual callback sources if they have overloads for coroutine callbacks.)

2. The rclcpp::CallbackGroup used must be reentrant [1].

In the following listing, you can see the constructor of the example node. Firstly, we initialize the parent node, a local copy of the logger, the required reentrant callback group and the service client. Next we initialize the timer with our coroutine callback. For timer, we use a constructor overload taking mrs_lib::TimerHandlerOptions (contains node and callback group), rate and the callback. The coroutine callback is passed as two parts – the method pointer and a pointer to the instance with which it is invoked. Finally, we also initialize another timer to see the executor is not blocked during the service call.

Constructor of the example node.

CoroServiceExample(const rclcpp::NodeOptions& opts)
    : Node("coro_service_example", opts), logger(this_node().get_logger()),
      reentrant_callback_group_(this_node().create_callback_group(rclcpp::CallbackGroupType::Reentrant)),
      service_client_(this_node_ptr(), "my_service", nullptr),
      service_timer_(std::make_unique<mrs_lib::ROSTimer>(std::invoke([this] {
                                                           mrs_lib::TimerHandlerOptions opts{this_node_ptr()};
                                                           opts.callback_group = reentrant_callback_group_;
                                                           return opts;
                                                         }),
                                                         rclcpp::Rate(5s), &CoroServiceExample::service_timer_callback, this)),
      chatter_timer_(
          std::make_unique<mrs_lib::ROSTimer>(this_node_ptr(), rclcpp::Rate(1s), std::bind_front(&CoroServiceExample::chatter_timer_callback, this)))
{
}

How to run

The example is automatically built when building mrs_lib with tests enabled. Alternatively, you can copy both the example and CMakeLists.txt file from the repo to your package.

To run the example, you have to start both the server and the client. (Don’t forget to source your workspace.)

Running server.

$ ros2 run mrs_lib example_coro_service_server
[INFO] [...]: Created service: /my_service
[INFO] [...]: Received service call. Starting work...
[INFO] [...]: Work done. Sending response...
[INFO] [...]: Received service call. Starting work...
[INFO] [...]: Work done. Sending response...

Running client.

$ ros2 run mrs_lib example_coro_service
[INFO] [...]: Created client 'my_service' -> '/my_service'
[INFO] [...]: Chattering ... (0)
[INFO] [...]: Chattering ... (1)
[INFO] [...]: Chattering ... (2)
[INFO] [...]: Chattering ... (3)
[INFO] [...]: Calling service...
[INFO] [...]: Chattering ... (4)
[INFO] [...]: Chattering ... (5)
[INFO] [...]: Service response:
success: 'true'
message: 'Response to service with value: 'true''
[INFO] [...]: Chattering ... (6)
[INFO] [...]: Chattering ... (7)
[INFO] [...]: Chattering ... (8)
[INFO] [...]: Calling service...
[INFO] [...]: Chattering ... (9)
[INFO] [...]: Chattering ... (10)
[INFO] [...]: Service response:
success: 'true'
message: 'Response to service with value: 'true''
[INFO] [...]: Chattering ... (11)

From the output of the client, we can see that the chattering timer is running even during the service call. During the processing of the service call, the coroutine is suspended and thus it can handle the other callbacks, such as the chatter. When the service server finishes around a second later, the coroutine is resumed and it prints the response.

Full Source code

This is the full source code for the example. You can also find it in the examples directory of mrs_lib repo.

Full code for the example.

#include <std_srvs/srv/set_bool.hpp>
#include <mrs_lib/coro/task.hpp>
#include <mrs_lib/node.h>
#include <mrs_lib/service_client_handler.h>
#include <mrs_lib/service_server_handler.h>
#include <mrs_lib/timer_handler.h>


namespace mrs_lib_examples
{
  using namespace std::chrono_literals;
  using ServiceType = std_srvs::srv::SetBool;

  class CoroServiceExample : public mrs_lib::Node
  {
  public:
    // BEGIN CTOR
    CoroServiceExample(const rclcpp::NodeOptions& opts)
        : Node("coro_service_example", opts), logger(this_node().get_logger()),
          reentrant_callback_group_(this_node().create_callback_group(rclcpp::CallbackGroupType::Reentrant)),
          service_client_(this_node_ptr(), "my_service", nullptr),
          service_timer_(std::make_unique<mrs_lib::ROSTimer>(std::invoke([this] {
                                                               mrs_lib::TimerHandlerOptions opts{this_node_ptr()};
                                                               opts.callback_group = reentrant_callback_group_;
                                                               return opts;
                                                             }),
                                                             rclcpp::Rate(5s), &CoroServiceExample::service_timer_callback, this)),
          chatter_timer_(
              std::make_unique<mrs_lib::ROSTimer>(this_node_ptr(), rclcpp::Rate(1s), std::bind_front(&CoroServiceExample::chatter_timer_callback, this)))
    {
    }
    // END CTOR

  private:
    // BEGIN CORO
    mrs_lib::Task<std::optional<std::shared_ptr<ServiceType::Response>>> call_service(bool data)
    {
      auto request = std::make_shared<ServiceType::Request>();
      request->data = data;
      co_return co_await service_client_.callAwaitable(request);
    }
    // END CORO

    // BEGIN CORO CALLBACK
    mrs_lib::Task<> service_timer_callback()
    {
      RCLCPP_INFO_STREAM(logger, "Calling service...");
      std::optional<std::shared_ptr<ServiceType::Response>> response_opt = co_await call_service(true);
      if (response_opt.has_value())
      {
        auto response = response_opt.value();
        std::string response_str = std::format("success: '{}'\n  message: '{}'", response->success, response->message);
        RCLCPP_INFO_STREAM(logger, "Service response:\n" << response_str);
      } else
      {
        RCLCPP_WARN_STREAM(logger, "Failed to call service!!!");
      }
    }
    // END CORO CALLBACK


    void chatter_timer_callback()
    {
      RCLCPP_INFO_STREAM(logger, "Chattering ... (" << message_number_ << ")");
      message_number_ += 1;
    }

    rclcpp::Logger logger;

    size_t message_number_ = 0;

    std::shared_ptr<rclcpp::CallbackGroup> reentrant_callback_group_;
    mrs_lib::ServiceClientHandler<ServiceType> service_client_;
    std::unique_ptr<mrs_lib::MRSTimer> service_timer_;
    std::unique_ptr<mrs_lib::MRSTimer> chatter_timer_;
  };


  class CoroServiceExampleServer : public mrs_lib::Node
  {
  public:
    CoroServiceExampleServer(const rclcpp::NodeOptions& opts)
        : Node("coro_service_example_server", opts), logger(this_node().get_logger()),
          service_server_(this_node_ptr(), "my_service", std::bind_front(&CoroServiceExampleServer::service_callback, this))
    {
    }

  private:
    void service_callback(std::shared_ptr<ServiceType::Request> req, std::shared_ptr<ServiceType::Response> res)
    {
      RCLCPP_INFO_STREAM(logger, "Received service call. Starting work...");

      // Simulated work...
      // Warning: This blocks the executor.
      // If this runs on a single threaded executor, no other callbacks
      // on ANY node in this executor will be called until this one finishes.
      // In the case of this example, it is ok, but you should be aware of this problem.
      std::this_thread::sleep_for(1s);
      res->success = true;
      res->message = std::format("Response to service with value: '{}'", req->data);

      RCLCPP_INFO_STREAM(logger, "Work done. Sending response...");
    }

    rclcpp::Logger logger;

    mrs_lib::ServiceServerHandler<ServiceType> service_server_;
  };

} // namespace mrs_lib_examples

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(mrs_lib_examples::CoroServiceExample)
RCLCPP_COMPONENTS_REGISTER_NODE(mrs_lib_examples::CoroServiceExampleServer)

Footnotes

[1]

The reentrant callback group requirement is a caused by limitations when working with rclcpp callback groups. When the coroutine gets suspended, the calling function exits and thus the callback looks finished to rclcpp, thus unlocking the callback group. Since it is not possible to keep the group locked, and another callback may start, reentrant callback groups requirement is there to expose this to the user.