Method RosDataProvider

src/ros-data-source.cpp:20–79

namespace VIO {

RosDataProvider::RosDataProvider()
    : RosBaseDataProvider(),
      left_img_subscriber_(),
      right_img_subscriber_(),
      sync_(nullptr),
      // initialize last timestamp (img) to be 0
      last_timestamp_(0),
      // initialize last timestamp (imu) to be 0
      last_imu_timestamp_(0),
      // keep track of number of frames processed
      frame_count_(1) {
  ROS_INFO("Starting SparkVIO wrapper for online");

  // Start IMU subscriber
  imu_subscriber_ =
      nh_imu_.subscribe("imu", 50, &RosDataProvider::callbackIMU, this);

  // Define Callback Queue for IMU Data
  ros::CallbackQueue imu_queue;
  nh_imu_.setCallbackQueue(&imu_queue);

  // Spawn Async Spinner (Running on Custom Queue) for IMU
  // 0 to use number of processor queues

  ros::AsyncSpinner async_spinner_imu(0, &imu_queue);
  async_spinner_imu.start();

  // Subscribe to stereo images. Approx time sync, should be exact though...
  DCHECK(it_);
  left_img_subscriber_.subscribe(*it_, "left_cam", 1);
  right_img_subscriber_.subscribe(*it_, "right_cam", 1);
  sync_ = VIO::make_unique<message_filters::Synchronizer<sync_pol>>(
      sync_pol(10), left_img_subscriber_, right_img_subscriber_);
  DCHECK(sync_);
  sync_->registerCallback(
      boost::bind(&RosDataProvider::callbackCamAndProcessStereo, this, _1, _2));

  // Define Callback Queue for Cam Data
  ros::CallbackQueue cam_queue;
  nh_cam_.setCallbackQueue(&cam_queue);

  // Spawn Async Spinner (Running on Custom Queue) for Cam
  ros::AsyncSpinner async_spinner_cam(0, &cam_queue);
  async_spinner_cam.start();

  ////// Define Reinitializer Subscriber
  reinit_flag_subscriber_ = nh_reinit_.subscribe(
      "reinit_flag", 10, &RosDataProvider::callbackReinit, this);
  reinit_pose_subscriber_ = nh_reinit_.subscribe(
      "reinit_pose", 10, &RosDataProvider::callbackReinitPose, this);

  // Define Callback Queue for Reinit Data
  ros::CallbackQueue reinit_queue;
  nh_reinit_.setCallbackQueue(&reinit_queue);

  ros::AsyncSpinner async_spinner_reinit(0, &reinit_queue);
  async_spinner_reinit.start();