Skip to content
/ icey Public

ICEY: A new API for the Robot Operating System (ROS) 2 for modern asynchronous programming using C++ 20's async/await

License

Notifications You must be signed in to change notification settings

iv461/icey

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

Project Logo


ROS 2 Jazzy Build Status ROS 2 Humble Build Status

ICEY is a new client API for modern asynchronous programming in the Robot Operating System (ROS) 2. It uses C++20 coroutines with async/await syntax for service calls and TF lookups. ICEY allows you to model data flows based on streams and promises. These features simplify application code and make asynchronous data flows clearly visible.

Problems ICEY solves:

  • Deadlocks are impossible since there is no need to manually spin the ROS executor (event loop) inside callbacks.
  • There are no memory leaks during service calls — every request is cleaned up automatically after the specified timeout.
  • All callbacks can be asynchronous functions (i.e., coroutines), which makes it possible to call and co_await other asynchronous operations inside callbacks.
  • A consistent async/await-based API for all asynchronous operations, including service calls and TF lookup.
  • Topic synchronization without boilerplate code

ICEY is fully compatible with the ROS 2 API since it is built on top of rclcpp. This allows for gradual adoption. It supports all major ROS features: parameters, subscriptions, publishers, timers, services, clients, and TF. Additionally, ICEY supports lifecycle nodes using a single API. ICEY operates smoothly with the message_filters package, using it for synchronization. ICEY is also extensible, as demonstrated by its support for image transport camera subscription/publishers.

ICEY supports ROS 2 Humble and ROS 2 Jazzy.

The icey_examples package contains many different example nodes, demonstrating the capabilities of ICEY.

Documentation

https://iv461.github.io/icey

Features

Using ICEY

To use ICEY in an existing Node class, you simply add an icey::Context to it and initialize it in the constructor:

#include <icey/icey.hpp>

class MyNode: public rclcpp::Node {

  MyNode() : rclcpp::Node("my_node") {
    icey_context_ = std::make_shared<icey::Context>(this);
  }

  std::shared_ptr<icey::Context> icey_context_;  
};

Service calls using async/await

Service calls can be awaited, a timeout has to be specified (no manual cleanup of pending requests required):

auto service = icey_context_->create_client<ExampleService>("set_bool_service");

ctx.create_timer(1s, [&]() -> icey::Promise<void> {
        /// Build a request each time the timer ticks
        auto request = std::make_shared<ExampleService::Request>();
        request->data = true;

        /// Call the service (asynchronously) and await the response with 1 second timeout:
        auto result = co_await service.call(request, 1s);
        
        if (result.has_error()) {
            /// Handle errors: (possibly "TIMEOUT" or "INTERRUPTED")
            RCLCPP_INFO_STREAM(node->get_logger(), "Got error: " << result.error());
        } else {
            RCLCPP_INFO_STREAM(node->get_logger(), "Got response: " << result.value()->success);
        }
        co_return;
    })

See also the Service client example.

Asynchronous service server callbacks:

With ICEY, you can use asynchronous functions, i.e., coroutines, as callbacks for both subscribers and services. For example, you can use them to call another upstream service.

/// Here using icey::Node that contains an icey::Context
auto upstream_service_client =
      icey_context_->create_client<ExampleService>("set_bool_service_upstream");

icey_context_->create_service<ExampleService>(
      "set_bool_service", 
        /// An asynchronous callback (coroutine) returns a Promise<Response>:
        [&](auto request) -> icey::Promise<Response> {

        /// Call the upstream service with 1s timeout asynchronously:
        auto upstream_result = co_await upstream_service_client.call(request, 1s);

        if (upstream_result.has_error()) {
          RCLCPP_INFO_STREAM(node->get_logger(),
                             "Upstream service returned error: " << upstream_result.error());
          /// Return nothing: This will simply not respond to the client, leading to a timeout
          co_return nullptr;
        } else {
          Response upstream_response = upstream_result.value();
          /// Respond to the client with the upstream server's response:
          co_return upstream_response;
        }
        
      });

See also the Service server example.

TF lookup using async/await

  icey::TransformBuffer tf_buffer = icey_context_->create_transform_buffer();

  icey_context_->
      // Use a coroutine (an asynchronous function) as a callback for the subscription:
      .create_subscription<sensor_msgs::msg::PointCloud2>("point_cloud", [&tf_buffer,
             &node](sensor_msgs::msg::PointCloud2::SharedPtr point_cloud) -> icey::Promise<void> {
        icey::Result<geometry_msgs::msg::TransformStamped, std::string> tf_result =
            co_await tf_buffer.lookup("map", point_cloud->header.frame_id,
                                      point_cloud->header.stamp, 200ms);

        if (tf_result.has_value()) {
          geometry_msgs::msg::TransformStamped transform_to_map = tf_result.value();
          Eigen::Matrix4d tf_mat = tf2::transformToEigen(transform_to_map.transform).matrix();
          RCLCPP_INFO_STREAM(node->get_logger(), "Got transform:\n",
                      tf_mat);
        } else {
          RCLCPP_INFO_STREAM(node->get_logger(), "Transform lookup error " << tf_result.error());
        }
        co_return;
      });

See also the TF Async/await example example.

Async Flow:

Synchronize an arbitrary number of topics easily (using the approximate time policy):

auto camera_image = icey_context_->create_subscription<sensor_msgs::msg::Image>("camera");
auto point_cloud = icey_context_->create_subscription<sensor_msgs::msg::PointCloud2>("point_cloud");

  /// Synchronize by approximately matching the header time stamps (queue_size=100):
  icey::synchronize_approx_time(100, camera_image, point_cloud)
      .then([](sensor_msgs::msg::Image::SharedPtr,
               sensor_msgs::msg::PointCloud2::SharedPtr) {
      });

See also the Synchronization example.

Synchronizer topics with a transform:

  auto camera_image = icey_context_->create_subscription<sensor_msgs::msg::Image>("camera");
  camera_image.synchronize_with_transform("map", 100ms)
      .then([](sensor_msgs::msg::Image::SharedPtr image,
               const geometry_msgs::msg::TransformStamped &transform_to_map) {
      });

See also the TF Synchronization example.

Detecting timeouts on topics:

  icey_context_->create_subscription<geometry_msgs::PoseStamped>("ego_pose")
    /// Expect that every pose message is at most 200ms old
    .timeout(200ms)
    /// "unwrap_or" handles calls the provided callback in case of error
    .unwrap_or([&](auto current_time, auto msg_time, auto max_age) {
        auto msg_dt = (current_time - message_timestamp).seconds();
        RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, fmt::format(
            "Timeout occured, message is {} seconds old, maximum allowed is {} seconds",
            msg_dt, max_age));
    }) 
    .then([](geometry_msgs::PoseStamped::SharedPtr pose_msg) {
      /// All message received here are at most 200ms old
    });

Filter messages based on a predicate:

icey_context_->create_subscription<geometry_msgs::PoseStamped>("ego_pose")
    /// Filter messages containing NaNs:
    .filter([](geometry_msgs::PoseStamped::SharedPtr pose_msg) -> bool {
        return !(std::isnan(pose_msg->pose.x) 
                  ||std::isnan(pose_msg->pose.y) 
                  || std::isnan(pose_msg->pose.z));
    })
    .then([](geometry_msgs::PoseStamped::SharedPtr pose_msg) {
      /// All messages received here are NaN-free
    });

Dependencies:

  • C++20
  • ROS 2 Humble or Jazzy
  • Boost (Hana, typeinfo)
  • FMT

Install with: sudo apt install libfmt-dev libboost-dev.

Note that ROS 2 Humble is as of now (April 2025) already forward compatible with C++20 (compiling ROS-headers with -std=c++20): Information you will find online stating the contrary is simply outdated.

Install

Just clone this repository to your workspace, install dependencies and compile:

sudo apt install libboost-dev libfmt-dev
MAKEFLAGS="-j4" colcon build --packages-select icey icey_examples --cmake-args -DCMAKE_BUILD_TYPE=Release

Note: Use MAKEFLAGS="-j4" to prevent you system from freezing.

Explaination

By default, colcon will start as many compiler processes as there are CPU cores, if there are enough translation units (TU) to compile in parallel. The icey_example package contains ~20 examples and therefore TUs. Since GCC requires 1-3 GiB of RAM to compile a single TU using icey, on a machine with 20 CPU cores (such as a 12th generation Intel i7) and only 32 GiB of RAM, this will require more RAM than is available. So Linux starts swapping, which takes a very long time because at the same time the CPU load is also high. The result is an effectively unresponsive system. Linux has an out-of-memory killer (OOM killer), but by default it is configured to be effectively useless, it won't kill the GCC processes. By passing the option MAKEFLAGS="-j4", only four jobs will be used, i.e. only 4 TUs will be compiled in parallel. This will prevent your system from freezing assuming you have at least 12 GiB of RAM. Of course (and after you read this far) you can set it to whatever value you like. We just want to prevent your first experience with ICEY from being "it freezes your system and you have to reboot", which would be very unpleasant.

To depend on ICEY in your ROS package, simply add <depend>icey</depend> to your package.xml and include icey/icey.hpp in your node.

Limitations

Our goal with ICEY is to support everything that ROS supports. However, there are a few minor limitations:

  • Not thread-safe: only the SingleThreadedExecutor is supported currently
  • Memory strategy is not implemented
  • Sub-nodes are not supported

License

This software is licensed under the Apache License, Version 2.0.

Acknowledgements

This project has received research funding from the german Bundesministerium für Wirtschaft und Energie, during the project RDV - Real Drive Validation - Erweiterung der Überprüfbarkeit von Continuous Software Integration in Kommunikation mit Fahrzeugen im Feld, Teilvorhaben Sicherheitsnachweis im Entwicklungszyklus für hochautomatisierte Fahrzeuge, grant number 19A21051K.

About

ICEY: A new API for the Robot Operating System (ROS) 2 for modern asynchronous programming using C++ 20's async/await

Topics

Resources

License

Stars

Watchers

Forks

Packages

No packages published