Skip to content

[Proposal] Adding single message getting API to rclcpp #1953

@SteveMacenski

Description

@SteveMacenski

Problem statement:

We're missing a way to get a single message from a topic. This question has come up a number of times in conversations with my colleagues, ROS Answers question askers, and myself even noticing the absense of this API. ROS 1 had this as waitForMessage().

An example use-case is how I used it in ROS 1 in SLAM Toolbox: when a service requests deserialization of a previous SLAM session, I want to get a single message from the current laser scanner to match it to the serialized one to ensure compatibility (else throw exception). Another example mentioned to me by a colleague was getting a single message from the camera info topic from a fixed-focus camera since it never changes to store and have the input to a machine learning system.

There are plenty of examples of a need for a way to get a message from a topic in a polling / blocking manner. I don't think that it is wise for architectural reasons to lean on this kind of method for regular polling of topics and should be advised in documentation that it is used as only when the odd situation requires it. As such, we don't need to be excessively concerned with performance but it shouldn't be outrageously bad (impl proposal below is naive but fine from that perspective).

For better performance or for use of regular polling, it is certainly possible to create subscriptionPolls, or a wrapper on subscriptions that handle message gathering and contain a method getLastMessage() to grab the last message if the programming style for regular polling is desirable in an efficient way. I think that is out of scope of this particular feature request but also could be useful for long-running subscriber/regular polling needs (though easy and common for users to bake into their nodes).

Proposed solution:

I propose adding a top-level rclcpp API called getSingleMessage or getLatestMessage (or similar) which will create a subscription within the function and block until a message is recieved or a timeout expires. An example can be seen below:

template <typename MsgT, NodeT = rclcpp::Node>
MsgT::SharedPtr rclcpp::getSingleMessage<MsgT, NodeT>(
  NodeT node,
  const std::string & topic,
  const rclcpp::Duration & timeout_dur = rclcpp::Duration(0),
  const rmw_qos_profile_t qos_profile = rmw_qos_profile_default,
  const rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr)
{
  rclcpp::Time start = node->now();
  MsgT::SharedPtr my_msg;
  auto sub = node->create_subscription<MsgT>(topic, [&](const MsgT::SharedPtr msg){my_msg = msg;}, QoS, callback_group);

  while (node->now() - start < timeout_dur) {
    rclcpp::spin_some(node);
    if (my_msg) {
      sub.reset();
      return my_msg;
    }
  }

  sub.reset();
  throw std::runtime_error("Some error msg");
}

Elements I think are most important:

  • Ability to have a timeout for maximum time to block
  • Setting QoS / callback group to use for subscription (optionally)
  • Templated based on node type so it can support both rclcpp::Node and rclcpp_lifecycle::LifecycleNode (and others in the future)
  • Handled in the client library so users can easily access rather than baking the above logic in-line in application code that can be easily generalized

Documentation and other considerations:

  • Should be for isolated or single use, not polling for a topic. This method is inefficient for regular use and not meant for that.
  • Should we add in executor internally so doesn't disrupt program flow? I don't care either way, but seems like it might be excessive but happy to do it if we feel that's the right direction to replace callback groups.

Metadata

Metadata

Assignees

No one assigned

    Labels

    enhancementNew feature or requesthelp wantedExtra attention is needed

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions