0
$\begingroup$

Rosanswers logo

I'm trying to sync up some behaviors and some data while avoiding callback hell. What's a good way to effectively wait for a new data on a subscription in a synchronous manner?

Here's the high level of what I'd like to do:

start_process_callback() {
    trigger_led();
    wait_seconds();
    leftimage = get_left_image();
    trigger_different_led();
    rightimage = get_right_image();
    result = do_things_to_images(leftimage, rightimage);
    send_result(result);
}

Where the LED functions are publishers and the get_image functions block until a new image is received on a topic then return that image. I hope this helps clarify things. ROS1 had a wait_for_message, but ROS2 doesn't seem to have anything like that. The only other alternative I can think of is to have a convoluted state machine in a bunch of nested callbacks, but I'd really rather not do that if possible.

Thoughts?


Originally posted by talos on ROS Answers with karma: 25 on 2018-10-31

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

There's no way to do it right now, but we want to change that, see: https://github.com/ros2/rclcpp/issues/520 You can use the C API directly, however. It has a wait set which can wait on one or more subscriptions to be ready and then you can take from them in any order.


Originally posted by William with karma: 17335 on 2018-10-31

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by talos on 2018-11-01:
Thanks for the link! I notice that this says how to wait without an executor, are there any examples about how to wait with an executor?

Comment by William on 2018-11-01:
No, but the solution to that issue I linked to might be that the executor can either spin or wait. Not sure yet.

I know some people are doing it but they've created their own Subscription class in C++ to accomplish it, which isn't ideal.

Comment by William on 2018-11-01:
If you really want to do it now, perhaps this function from the rcl tests will be helpful: https://github.com/ros2/rcl/blob/77ef88a4f6b8af70d3f6a7443ba00c3981a0b559/rcl/test/rcl/test_subscription.cpp#L64-L98

Comment by talos on 2018-11-04:
We've temporarily set it up to spin in an executor with multiple threads and manual flags. Not an elegant solution, but it works fine for now.

Comment by bvaningen on 2021-05-12:
What is the status of this now in 2021? I have so far not been able to finding anything on it, so i guess it is still not implemented?

Comment by Karsten on 2021-06-30:
that worked for me in Foxy. Not vouching for any correctness here and this should most likely be added to subscription class itself upstream:

rclcpp::WaitSet wait_set;
wait_set.add_subscription(subscription);
auto ret = wait_set.wait(std::chrono::seconds(10));
if (ret.kind() == rclcpp::WaitResultKind::Ready) {
  std_msgs::msg::String msg;
  rclcpp::MessageInfo info;
  auto ret_take = subscription->take(msg, info);
  if (ret_take) {
    RCLCPP_INFO(this->get_logger(), "got message: %s", msg.data.c_str());
  } else {
    RCLCPP_ERROR(this->get_logger(), "no message received");
  }
} else {
  RCLCPP_ERROR(this->get_logger(), "couldn't wait for message");
}
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.