Entities#

All the ROS entities (subscriptions, publishers, timers etc.) are wrapped so that they are either Streams or provide an asynchronous interface with icey::Promise (service client, TF).

template<class ServiceT>
struct ServiceClient#

A service client offering an async/await API and per-request timeouts. Service calls happen asynchronously and return a promise that can be awaited using co_await. This allows synchronization with other operations, enabling you to effectively perform synchronous service calls. We do not offer a function to wait until the service is available for two reasons: (1) there is no asynchronous function available at the rclcpp layer (re-implementation using asynchronous graph change notification would be required), and (2) ROS does not seem to optimize RPC calls by keeping connections open; therefore, it does not seem beneficial to differentiate whether the server is available or not.

Public Types

using Request = typename ServiceT::Request::SharedPtr#
using Response = typename ServiceT::Response::SharedPtr#
using RequestID = decltype(rclcpp::Client<ServiceT>::FutureAndRequestId::request_id)#

The RequestID in rclcpp is currently of type int64_t.

Public Functions

ServiceClient(std::weak_ptr<NodeBase> node, const std::string &service_name, const rclcpp::QoS &qos = rclcpp::ServicesQoS())#

Constructs the service client. A node has to be provided because it is needed to create timeout timers for every service call.

impl::Promise<Response, std::string> call(Request request, const Duration &timeout) const#

Make an asynchronous call to the service. Returns a impl::Promise that can be awaited using co_await

. Requests can never hang forever but will eventually time out. Also you don’t need to clean up pending requests &#8212; they will be cleaned up automatically. So this function will never cause any memory leaks.

Example usage:

  auto client = node->icey().create_client<ExampleService>("set_bool_service1");
  auto request = std::make_shared<ExampleService::Request>();
  icey::Result<ExampleService::Response::SharedPtr, std::string> result = co_await client.call(request, 1s);

Parameters:
  • request – the request

  • timeout – The timeout for the service call, both for service discovery and the actual call.

Returns:

A promise that can be awaited to obtain the response or an error. Possible errors are “TIMEOUT”, “SERVICE_UNAVAILABLE” or “INTERRUPTED”.

std::shared_ptr<rclcpp::Client<ServiceT>> client() const#

Returns the underlying rclcpp service client.

struct TransformBuffer#

A TransformBuffer offers a modern, asynchronous interface for looking up transforms at a given point in time. This class is lightweight pointer to underlying implementation and can therefore be copied and passed around by value (PIMPL idiom)

Public Types

using RequestHandle = TransformBufferImpl::RequestHandle#
using OnTransform = std::function<void(const geometry_msgs::msg::TransformStamped&)>#
using OnError = std::function<void(const tf2::TransformException&)>#

Public Functions

TransformBuffer() = default#
TransformBuffer(std::shared_ptr<TransformBufferImpl> impl)#
RequestHandle lookup(const std::string &target_frame, const std::string &source_frame, Time time, const Duration &timeout, OnTransform on_transform, OnError on_error)#

Makes an asynchronous lookup for a for a transform at the given time between the given frames. Callback-based API.

Warning

Currently the timeout is measured with wall-time, i.e. does not consider sim-time due to the limitation of ROS 2 Humble only offering wall-timers

Parameters:
  • target_frame

  • source_frame

  • time

  • timeout

  • on_transform – The callback that is called with the requested transform after it becomes available

  • on_error – The callback that is called if a timeout occurs.

Returns:

The “handle”, identifying this request. You can use this handle to call cancel_request if you want to cancel the request.

bool cancel_request(RequestHandle request_handle)#

Cancel a transform request: This means that the registered callbacks will no longer be called. If the given request does not exist, this function does nothing, in this case it returns false. If a request was cleaned up, this function returns true.

impl::Promise<geometry_msgs::msg::TransformStamped, std::string> lookup(const std::string &target_frame, const std::string &source_frame, const Time &time, const Duration &timeout)#

Does an asynchronous lookup for a single transform that can be awaited using co_await

Parameters:
  • target_frame

  • source_frame

  • time – At which time to get the transform

  • timeout – How long to wait for the transform

Returns:

A future that resolves with the transform or with an error if a timeout occurs

impl::Promise<geometry_msgs::msg::TransformStamped, std::string> lookup(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time, const Duration &timeout)#

Same as lookup, but accepts a ROS time point.

Parameters:
  • target_frame

  • source_frame

  • time – At which time to get the transform

  • timeout – How long to wait for the transform

Returns:

A future that resolves with the transform or with an error if a timeout occurs

template<class ActionT>
struct ActionClient#

A action client offering an async/await API and per-request timeouts. Everything happens asynchronously and returns a promise that can be awaited using co_await.

Public Types

using Goal = typename ActionT::Goal#
using Client = icey::rclcpp_action::Client<ActionT>#
using GoalHandle = icey::rclcpp_action::ClientGoalHandle<ActionT>#
using FeedbackCallback = typename GoalHandle::FeedbackCallback#
using Result = typename GoalHandle::WrappedResult#
using RequestID = int64_t#
using AsyncGoalHandleT = AsyncGoalHandle<ActionT>#

Public Functions

ActionClient(std::weak_ptr<NodeBase> node, const std::string &action_name)#
template<class F>
impl::Promise<AsyncGoalHandleT, std::string> send_goal(const Goal &goal, const Duration &timeout, F &&feedback_callback) const#

Send asynchronously an action goal: if it is accepted, then you get a goal handle.

template<class ActionT>
struct AsyncGoalHandle#

An AsyncGoalHandle is created once a requested goal was accepted by the action server. It provides an async/await based API for requesting the result and cancellation. WARNING: On Humble, there seems to be a memory leak bug in the underlying send_goal API.

Public Types

using Goal = typename ActionT::Goal#
using GoalHandle = icey::rclcpp_action::ClientGoalHandle<ActionT>#
using Result = typename GoalHandle::WrappedResult#
using Client = icey::rclcpp_action::Client<ActionT>#
using CancelResponse = typename Client::CancelResponse::SharedPtr#
using Feedback = typename Client::Feedback#
using ResultPromise = impl::Promise<Result, std::string>#

Public Functions

AsyncGoalHandle(std::weak_ptr<NodeBase> node, std::shared_ptr<Client> client, const std::shared_ptr<GoalHandle> &goal_handle)#
const rclcpp_action::GoalUUID &get_goal_id() const#

Get the goal UUID.

rclcpp::Time get_goal_stamp() const#

Get the time when the goal was accepted.

int8_t get_status()#

Get the goal status code. See https://docs.ros.org/en/ros2_packages/rolling/api/action_msgs/msg/GoalStatus.html for possible values

ResultPromise result(const Duration &timeout) const#

Obtain the result asynchronously.

impl::Promise<CancelResponse, std::string> cancel(const Duration &timeout) const#

Cancel this goal. Warning: Not co_awaiting this promise leads to use-after free !

TODO check in gdb whether it is a problem that this promise might resolve synchronously, i.e. do we get a stack overflow ?

std::shared_ptr<GoalHandle> get_goal_handle() const#

Returns the held rclcpp_action::GoalHandle.

template<class _Value>
struct ParameterStream : public icey::Stream<_Value>#

An stream representing parameters. It always registers the callback to obtain the parameter updates.

Public Types

using Base = Stream<_Value>#
using Value = _Value#

Public Functions

ParameterStream() = default#
ParameterStream(const Value &default_value, const Validator<Value> &validator = {}, std::string description = "", bool read_only = false, bool ignore_override = false)#

A constructor that should only be used for parameter structs. It does not set the name of the parameter and therefore leaves this ParameterStream in a not fully initialized state. Context::declare_parameter_struct later infers the name of this parameter from the name of the field in the parameter struct and sets it before registering the parameter with ROS.

Parameters:
  • default_value

  • validator – the validator implementing constraints

  • description – the description written in the ParameterDescriptor

  • read_only – if yes, the parameter cannot be modified

  • ignore_override

ParameterStream(Context &context, const std::string &parameter_name, const Value &default_value, const Validator<Value> &validator = {}, std::string description = "", bool read_only = false, bool ignore_override = false)#

The standard constructor used when declaring parameters with Context::declare_parameter.

Parameters:
  • context – the ICEY context

  • parameter_name

  • default_value

  • validator – the validator implementing constraints

  • description

  • read_only – if set to true, parameter updates will be rejected

  • ignore_override

void register_with_ros(Context &context)#

Register this parameter with the ROS node, meaning it actually calls node->declare_parameter(). After calling this method, this ParameterStream will have a value.

const Value &value() const#

Get the value. Parameters are initialized always at the beginning, so they always have a value.

operator Value() const#

Allow implicit conversion to the stored value type for consistent API between constrained and non-constrained parameters when using the parameter structs.

Public Members

std::string parameter_name#
template<class _Message>
struct SubscriptionStream : public icey::Stream<_Message::SharedPtr, Nothing, SubscriptionStreamImpl<_Message>>#

A stream that represents a regular ROS subscription. It stores as its value always a shared pointer to the message.

Public Types

using Base = Stream<typename _Message::SharedPtr, Nothing, SubscriptionStreamImpl<_Message>>#
using Value = typename _Message::SharedPtr#
using SyncCallback = std::function<void(Value)>#
using AsyncCallback = std::function<Promise<void>(Value)>#

Public Functions

SubscriptionStream() = default#
SubscriptionStream(Context &context, const std::string &topic_name, const rclcpp::QoS &qos, const rclcpp::SubscriptionOptions &options)#
struct TimerStream : public icey::Stream<size_t, Nothing, TimerImpl>#

A Stream representing a ROS-Timer. It saves the number of ticks as it’s value. TODO use this TimerInfo thingy, it actually exists in ROS 2 as well.

Public Types

using Base = Stream<size_t, Nothing, TimerImpl>#

Public Functions

TimerStream() = default#
TimerStream(Context &context, const Duration &interval, bool is_one_off_timer)#
void reset()#
void cancel()#
template<class _Value>
struct PublisherStream : public icey::Stream<_Value, Nothing, PublisherImpl<_Value>>#

A Stream representing a ROS-publisher.

Template Parameters:

_Value – Can be either a Message or std::shared_ptr<Message> where Message must be something publishable, i.e. either a valid ROS message or a masqueraded type.

Public Types

using Base = Stream<_Value, Nothing, PublisherImpl<_Value>>#
using Message = remove_shared_ptr_t<_Value>#

Public Functions

PublisherStream() = default#
template<AnyStream Input = Stream<_Value>>
PublisherStream(Context &context, const std::string &topic_name, const rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(), const rclcpp::PublisherOptions publisher_options = {}, Input *maybe_input = nullptr)#
void publish(const _Value &message) const#
struct TransformSubscriptionStream : public icey::Stream<geometry_msgs::msg::TransformStamped, std::string, TransformSubscriptionStreamImpl>#

A Stream that represents a subscription between two coordinate systems. (See TFListener) It yields a value each time the transform between these two coordinate systems changes.

Public Types

using Base = Stream<geometry_msgs::msg::TransformStamped, std::string, TransformSubscriptionStreamImpl>#
using Message = geometry_msgs::msg::TransformStamped#
using Self = TransformSubscriptionStream#

Public Functions

TransformSubscriptionStream() = default#
TransformSubscriptionStream(Context &context, ValueOrParameter<std::string> target_frame, ValueOrParameter<std::string> source_frame)#
struct TransformPublisherStream : public icey::Stream<geometry_msgs::msg::TransformStamped, Nothing, TransformPublisherStreamImpl>#

Public Types

using Base = Stream<geometry_msgs::msg::TransformStamped, Nothing, TransformPublisherStreamImpl>#
using Value = geometry_msgs::msg::TransformStamped#

Public Functions

TransformPublisherStream() = default#
template<AnyStream Input = Stream<geometry_msgs::msg::TransformStamped>>
TransformPublisherStream(Context &context, Input *input = nullptr)#
void publish(const geometry_msgs::msg::TransformStamped &message)#

Publish a single message.

Parameter validation#

Some extra classes related to parameters:

template<class Value>
struct Interval#

What follows, is an improved parameters API. For API docs, see https://docs.ros.org/en/jazzy/p/rcl_interfaces First, some constraints we can impose on parameters: A closed interval, meaning a value must be greater or equal to a minimum value and less or equal to a maximum value.

Public Functions

Interval(Value _minimum, Value _maximum)#

Public Members

Value minimum#
Value maximum#
template<class Value>
struct Set#

A set specified by a given list of values that are in the set.

Public Functions

Set(std::vector<Value> l)#

Public Members

std::unordered_set<Value> set_of_values#
template<class Value>
struct Validator#

A parameter validator, validating parameter updates. It is able to constrain parameter values. It is essentially a function that returns true if a value is allowed and false otherwise.

Template Parameters:

Value – the parameter type (i.e. double)

Public Types

using ROSValue = std::conditional_t<std::is_unsigned_v<Value>, int, Value>#
using Validate = std::function<std::string(const rclcpp::Parameter&)>#

The type of the validator predicate, meaning the function that returns an error if the parameter update is rejected and an empty string otherwise. Why we don’t return a std::optional<std::string> ? Because this way we can ensure through the type system that if a parameter update is rejected, it is always rejected for a reason.

Public Functions

Validator()#

Allow default-constructed validator, by default allowing all values.

explicit Validator(const Validate &validate)#

Construct explicitly from a validation function (predicate).

Validator(const Interval<Value> &interval)#

Allow implicit conversion from some easy sets:

Validator(const Set<Value> &set)#

Implicit conversion from a Set of values.

Validate get_default_validator() const#

Public Members

rcl_interfaces::msg::ParameterDescriptor descriptor#
Validate validate#

A predicate that indicates whether a given value is feasible. By default, a validator always returns true since the parameter is unconstrained

template<class Value>
struct ValueOrParameter#

A class that abstracts a plain value and a ParameterStream so that both are supported in promise mode. This is needed for example for timeouts and coordinate system names.

Public Functions

ValueOrParameter() = default#
ValueOrParameter(const Value &value)#

Construct a ValueOrParameter implicitly from a value.

template<class T>
ValueOrParameter(const T &v)#

Construct a ValueOrParameter implicitly from a something similar to a value, For example, "hello" is a const char[5] literal that is convertible to a std::string, the value.

ValueOrParameter(const ParameterStream<Value> &param)#

Construct a ValueOrParameter implicitly from parameter (i.e. ParameterStream)

Public Members

std::function<Value()> get#

We use a std::function for the type erasure: When called, it obtains the current value.