ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

if condition for approximate time subscriber

asked 2019-06-09 12:03:32 -0500

rav728 gravatar image

In my current code I subscribe to two topics using the Approximate time policy and suppose the callback is called callbackTwoTopics.

Now there's this third that sometime published and sometimes doesn't publish, but when it does publish I'd like to call another function callbackThreeTopics instead of callbackTwoTopics.

I know how to subscribe to three topics, but I don't know how to have a condition on a callback. Is it possible to do this in ROS cpp? If so, how?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-06-09 16:17:04 -0500

JustinBlack02 gravatar image

updated 2019-06-10 21:35:23 -0500

I don't think there is a specific way to do that using the existing sync policies. To handle your case you would have to implement a new sync policy https://github.com/ros/ros_comm/tree/...

A simpler way would probably to only sync the two high frequency topics, buffer messages on the third topic and dequeue messages from the buffer in the two-topic callback, with some timestamp checking of course.

Edit: Here is some pseudocode to get you started but you need to put some effort into solving your problem instead of searching for an existing implementation. Please mark this question as answered if this has helped.

void processTwo(const T1 &t1, const T2 &t2);
void processThree(const T1 &t1, const T2 &t2, const T3 &t3);

std::deque<T3> buffer_t3;
const double sync_tolerance_seconds = 0.1;  // Some window of tolerance

void callbackTwoTopics(const T1 &t1, const T2 &t2) {
  if (buffer_t3.empty()) { 
    // You could also do timestamp checks here and decide to buffer your 
    // messages and wait for t3
    processTwo(t1, t2);
    return;
  }

  auto&& t3 = buffer_t3.front();
  ros::Time&& stamp1 = t1.header.stamp;
  ros::Time&& stamp3 = t3.header.stamp;
  double time_diff_seconds = stamp1.toSeconds() - stamp3.toSeconds();
  if (abs(time_diff_seconds) < sync_tolerance_seconds) {
    // t3 is usable
    buffer_t3.pop_front();
    processThree(t1, t2, t3);
  } else if (time_diff_seconds > 0) {
    // t3 is too far in the future, leave it in the buffer for later
  } else if (time_diff_seconds < 0) {
    // t3 is too far in the past, drop it
    buffer_t3.pop_front();
  }
}

void callbackOtherTopic(const T3 &t3) {
  buffer_t3.push_back(t3);
  process();
}
edit flag offensive delete link more

Comments

Thanks for your answer. I did a quick search on implementations of buffering and dequeue messages, but couldn't find too much. Would you be able to provide an example or point me to an example that does this? Also, what would function could you use to check timestamps when dequeuing messages?

rav728 gravatar image rav728  ( 2019-06-09 16:58:45 -0500 )edit

any help would be greatly appreciated @JustinBlack02 !

rav728 gravatar image rav728  ( 2019-06-10 11:21:37 -0500 )edit

It's impossible to clearly answer your question, you will have to figure out how precise of a solution you want on your own. I have edited the original answer with some pseudocode.

JustinBlack02 gravatar image JustinBlack02  ( 2019-06-10 21:33:32 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2019-06-09 12:01:00 -0500

Seen: 463 times

Last updated: Jun 10 '19