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

Best practice to check if subscriber has already received data

asked 2021-10-08 04:47:16 -0500

max11gen gravatar image

I want my timer to start exectuting its callback only after the node's two subscribers received data.

What I am currently doing is setting a bool variable to true in the subscribers' callbacks and then checking if both bools are true in the timer callback:

In the nodes CTOR:

    vehicle_position_sub_ =
            this->create_subscription<VehicleLocalPosition>(
                vehicle_prefix+"fmu/vehicle_local_position/out", 10,
                [this](const VehicleLocalPosition::UniquePtr msg) {
        vehicle_current_pos_=*msg;
        received_vehicle_pos_=true;
    });
    mission_sub_ =
            this->create_subscription<MissionWaypoints>(
                "gcs/mission_waypoints", 10,
                [this](const MissionWaypoints::UniquePtr msg){
        mission_waypoints_=*msg;
        received_setpoints_=true;
    });

And then in the timer callback:

if(received_vehicle_pos_&&received_setpoints_){
    ....
}

Now this doesn't seem efficient or clean to me since each subscriber callback will update the bool variables even though it's actually only needed in the very first time. And also the if-statement in the timer callback seems inefficient. Is there a better way to do this?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
2

answered 2021-10-08 05:17:17 -0500

morten gravatar image

updated 2021-10-08 05:21:27 -0500

As far as I am aware, ros 2 doesn't have a waitForMessage function like in ros1, that being said one can write a sort of workaround like

// waiting for first gps message to set the origin
RCLCPP_INFO(this->get_logger(), "Waiting for first gps message");
rclcpp::WaitSet wait_set;
wait_set.add_subscription(sub_);
auto ret = wait_set.wait(std::chrono::seconds(100));
if (ret.kind() == rclcpp::WaitResultKind::Ready) {
  sensor_msgs::msg::NavSatFix msg; // change as relevant
  rclcpp::MessageInfo info;
  auto ret_take = sub_->take(msg, info);
  if (ret_take) {
    RCLCPP_INFO(this->get_logger(), "heard gps, initializing offsets");
    /* do stuff */
  } else {
    RCLCPP_ERROR(this->get_logger(), "no message recieved from gps");
  }
} else {
  RCLCPP_ERROR(this->get_logger(), "couldn't wait for gps message");
  return;
}

Where sub_ is some previously defined subscription member variable of the node class. I figure you can wait for the messages in the class constructor before starting the timer loop.

Specifically this code I wrote to be particular to a gps message, but what it does in the /* do stuff */ section is up to you.

Note: I wrote/used this in a ros2 foxy pkg.

edit flag offensive delete link more

Comments

1

Sounds good. Especially, waiting in the CTOR and then starting the timer loop sounds like a clear optimization. If no easier solution comes up in the next few days, I'll mark this the solution.

max11gen gravatar image max11gen  ( 2021-10-08 05:38:40 -0500 )edit

@morten One question about this: why do you have to double check? Isn't it enough to check the return value of rclcpp::WaitSet::wait()?

max11gen gravatar image max11gen  ( 2021-10-08 08:40:35 -0500 )edit

From what I remember the auto ret is just defining a duration for which it waits to see if the topic exists. Afterwards there is a check for whether there is a message posted on the topic.

morten gravatar image morten  ( 2021-10-11 02:20:43 -0500 )edit

@max11gen did you find anything else out?

morten gravatar image morten  ( 2021-11-04 05:36:11 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2021-10-08 04:47:16 -0500

Seen: 694 times

Last updated: Oct 08 '21