RosOnlineDataProvider hangs when using sim_time

[violetteavi]

Description:
Somewhere along the line the RosOnlineDataProvider was limited to a 30 Hz publishing frequency via ros::Rate. Unfortunately, ros::Rate uses sim_time, which stops when a rosbag finishes. Any frame not published by that point gets effectively dropped. This is one or two frames if the VIO pipeline is running quickly, but many frames if VIO can't keep up with the publishing speed of the rosbag (I found out when my computer was low on battery and running suuuper slow).

Command:

roscore
roslaunch kimera_vio_ros kimera_vio_ros_euroc.launch
rviz -d $(rospack find kimera_vio_ros)/rviz/kimera_vio_euroc.rviz
rosbag play --clock /PATH/TO/EUROC_ROSBAG

Please give also the following information:

• SparkVio branch, tag or commit used: Master, master
• GTSAM version used:
• OpenGV version used:
• OpenCV version used: type opencv_version
• Operating system and version (e.g. Ubuntu 16.04 or Windows 10): Ubuntu 18.04
• Did you change the source code? (yes / no): No for the original problem. When I commented out the ros::Rate that fixed the problem.

[violetteavi]

We can consider using ros::WallRate instead. This ensures that the node keeps running no matter what; however, if the rosbag is being played at a different speed the 30Hz of wall time won't match up with the sim time.

Can we make publishing rate a launch parameter that defaults to 30?
I'll make a PR for it.

[ToniRV]

Good catch. I believe that if you keep spinning ROS, it will keep publishing even though the /clock topic does not get published anymore given that the rosbag stopped.

I would really discourage any use of WallRate clocks or any clock not using ros::Time because then we'll head directly to inconsistent timestamping and that is not a very nice world to be in...

[violetteavi]

Ok. What do you mean by ‘keep spinning ros?’ And how do you do it?

[ToniRV]

ros::spin() or ros::spinOnce().
I believe I was doing that already, once the rosbag would finish I would enter a while loop checking if ros was ok, and just spinning:

// Endless loop until ros dies to publish left-over outputs.
  while (nh_.ok() && ros::ok() && !ros::isShuttingDown() && !shutdown_) {
    FrontendOutput::Ptr frame_rate_frontend_output = nullptr;
    if (frame_rate_frontend_output_queue_.pop(frame_rate_frontend_output)) {
      publishFrontendOutput(frame_rate_frontend_output);
    }

    // Publish backend, mesher, etc output
    publishSyncedOutputs();

    LcdOutput::Ptr lcd_output = nullptr;
    if (lcd_output_queue_.pop(lcd_output)) {
      publishLcdOutput(lcd_output);
    }
  }

Although looking at this code, it seems like I was not actually spinning ROS but just publishing until VIO would stop processing.

[violetteavi]

It looks like there is an async spinner already devoted to spinning Ros as a whole, initialized at the end of the constructor.

  // This async spinner will process the regular Global callback queue of ROS.
  static constexpr size_t kGlobalSpinnerThreads = 2u;
  async_spinner_ = VIO::make_unique<ros::AsyncSpinner>(kGlobalSpinnerThreads);

[violetteavi]

The inside of that loop is what is currently inside spinOnce()

bool RosOnlineDataProvider::spinOnce() {
  // Publish frontend output at frame rate
  LOG_EVERY_N(INFO, 100) << "Spinning RosOnlineDataProvider.";
  FrontendOutput::Ptr frame_rate_frontend_output = nullptr;
  if (frame_rate_frontend_output_queue_.pop(frame_rate_frontend_output)) {
    publishFrontendOutput(frame_rate_frontend_output);
  }

  // Publish all output at keyframe rate (backend, mesher, etc)
  publishSyncedOutputs();

  // Publish lcd output at whatever frame rate it might go
  LcdOutput::Ptr lcd_output = nullptr;
  if (lcd_output_queue_.pop(lcd_output)) {
    publishLcdOutput(lcd_output);
  }
  return true;
}

spinOnce() is called inside of a rate-limited while loop:

  // Start our own spin to publish output data to ROS
  // Pop and send output at a 30Hz rate.
  ros::Rate rate(30);
  while (ros::ok() && !shutdown_) {
    rate.sleep();
    spinOnce();  // TODO(marcus): need a sequential mode?
  }

I suspect that ros::Rate is working as intended. If sim_time stops, then anything that moves in sim_time should also stop. Not sure how we can get around that if we don't want to introduce wall time.

[violetteavi]

It looks like wall time is the only way to have something keep running after sim_time stops (wiki).

[violetteavi]

The other alternative is to ditch the polling and go purely asynchronous using callbacks for completed backend outputs. If I understand correctly, that would tie the publishing into the ros::spin, which would allow it to continue after sim_time stops.

[violetteavi]

Fixed with the asynchronous callbacks in this PR. I have a video, but Git doesn't support MP4