Best practice to check if subscriber has already received data

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

max11gen
164 ●19 ●24 ●31

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?

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

morten
198 ●18 ●32 ●34

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

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

Comments

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 ( 2021-10-08 05:38:40 -0600 )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 ( 2021-10-08 08:40:35 -0600 )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 ( 2021-10-11 02:20:43 -0600 )edit

@max11gen did you find anything else out?

morten ( 2021-11-04 05:36:11 -0600 )edit

add a comment

Best practice to check if subscriber has already received data

1 Answer

Comments

Question Tools

Stats

Related questions

Best practice to check if subscriber has already received data edit

1 Answer

Comments

Question Tools

Stats

Related questions

Best practice to check if subscriber has already received data