ROS: Use lambda as callback in nodehandle.subscribe

user_6119162 0
Viewed 6806

I want to be able to define a callback through a lambda but I can't get any of the function signatures to match. 

std::function<void(const sensor_msgs::PointCloud2)> callback = 
 [&mycapture](const sensor_msgs::PointCloud2 msg) { 
 // do something with msg and capture
 };

auto sub = ros_node.subscribe("/sometopic", 1, callback)

I can't get this to work as subscribe wants a function pointer. Is what I'm trying to do possible with ROS? That is, can I pass into the subscribe method a lambda with a capture?

ros lambda c++
6 Answers

One-liner:

sensor_msgs::PointCloud2 pcl;
auto sub = nh.subscribe<sensor_msgs::PointCloud2>("scan", 1, [&](const sensor_msgs::PointCloud2ConstPtr& msg){pcl=*msg;});

Or explicit multi-liner utilizing the auto type which saves you lots of headaches with callbacks:

auto cb = [&](const sensor_msgs::PointCloud2ConstPtr& msg) {
  pcl=*msg;
};
auto sub = nh.subscribe<sensor_msgs::PointCloud2>("scan", 1, cb);
user_10765640

I have hit this issue as well in Melodic. While solution with explicit cast to boost function worked, it was very verbose. Under the hood, this solution boils to forcing compiler to choose the last overload of node_handle::subscribe that takes two template arguments:

   * \param M [template] the message type
   * \param C [template] the callback parameter type (e.g. const boost::shared_ptr<M const>& or const M&)

  template<class M, class C>
  Subscriber subscribe(const std::string& topic, uint32_t queue_size, const boost::function<void (C)>& callback

Because it is the only overload that has two template arguments, it can be enforced by specifying those parameters explicitly.

In specifics of the question asked, adding a subscriber will look like this (C++14):

ros::Subscriber subscriber = nh.subscribe<sensor_msgs::PointCloud2, const sensor_msgs::PointCloud2&>(topic, 1, [&mycapture](auto msg) {
    mycapture.workWith(msg.data);
  });
user_8885450

I was able to get this working with capture.

T obj;
boost::function<void (const sensor_msgs::PointCloud2&)> callback = 
    [obj] (const sensor_msgs::PointCloud2& msg) {
    // Do something here 
    }
auto sub = ros_nh.subscribe<sensor_msgs::PointCloud>(topic, 1, callback);

ROS Version: Indigo
C++14

user_25481650

The partial solution in the question seemed a bit cleaner without having a static cast.

This code works fine (also with timers, services, etc.):

boost::function<void (const sensor_msgs::PointCloud2&, CustomObject&)> callback =
    [] (const sensor_msgs::PointCloud2& msg, CustomObject& obj) {
    // do something with msg and obj
    }

ros::Subscriber = ros_nh.subscribe(topic, 1, boost::bind(callback, _1, obj));
user_31886540

Define a lambda, capture any obj references you need, and wrap the message my_msg in argument list.

auto lambda = [&obj](const ros::MessageEvent< my_msg const >& msg) {
    auto const_ref_msg = *(msg.getConstMessage().get());
    obj.callback(const_ref_msg);
};

auto sub = nh.subscribe< my_msg >("/my_msg", 1, lambda);

Caveats:

  • The message must be wrapped in ros::MessageEvent.
    • If lambda is declared to accept (my_msg msg) then compiler complains: "no known conversion for argument 1 from ‘const boost::shared_ptr<const my_msg_<std::allocator<void> > >’ to ‘my_msg {aka my_msg_<std::allocator<void> >}’"
  • Make sure you have auto subAck = so that the Subscriber remains on the stack frame. Otherwise compiler might optimize out the latter part mmNode.subscribe< my_msg >(my_msg::TOPIC, subscribeQueueSize, lambdaAck).
user_11056520
Related