Method Subscriber

okvis_ros/src/Subscriber.cpp:55–133

Subscriber::~Subscriber() { imgTransport_.release(); }

Subscriber::Subscriber(std::shared_ptr<rclcpp::Node> node,
                       okvis::VioInterface* vioInterfacePtr,
                       const okvis::VioParametersReader& param_reader)
    : node_(node), vioInterface_(vioInterfacePtr), imgTransport_(nullptr) {
  /// @Sharmin
  param_reader.getParameters(vioParameters_);

  imageSubscribers_.resize(vioParameters_.nCameraSystem.numCameras());

  imgTransport_ = std::make_unique<image_transport::ImageTransport>(node);

  // setup callback groups
  auto svin2_callback_group = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
  auto options = rclcpp::SubscriptionOptions();
  options.callback_group = svin2_callback_group;

  // Set up Camera callbacks
  for (size_t i = 0; i < vioParameters_.nCameraSystem.numCameras(); ++i) {
    imageSubscribers_[i] =
        imgTransport_->subscribe("camera" + std::to_string(i),
                                 30 * vioParameters_.nCameraSystem.numCameras(),
                                 std::bind(&Subscriber::imageCallback, this, std::placeholders::_1, i));
  }

  // Set up IMU callback
  subImu_ = node->create_subscription<sensor_msgs::msg::Imu>(
      "imu", 1000, std::bind(&Subscriber::imuCallback, this, std::placeholders::_1));
  //subImu_ = node->create_subscription<sensor_msgs::msg::Imu>(
  //    "imu", rclcpp::SensorDataQoS(), std::bind(&Subscriber::imuCallback, this, std::placeholders::_1), options);

  // Sharmin
  // if (vioParameters_.sensorList.isSonarUsed) {
  //   subSonarRange_ = nh_->subscribe("/imagenex831l/range", 1000, &Subscriber::sonarCallback, this);
  // }
  // Sharmin
  // if (vioParameters_.sensorList.isDepthUsed){
  // subDepth_ = nh_->subscribe("/bar30/depth", 1000, &Subscriber::depthCallback, this);
  // subDepth_ = nh_->subscribe("/aqua/state", 1000, &Subscriber::depthCallback, this); // Aqua depth topic
  // }

  // Sharmin
  // if (vioParameters_.relocParameters.isRelocalization) {
  //   std::cout << "Subscribing to /pose_graph/match_points topic" << std::endl;
  //   subReloPoints_ = node->create_subscription<sensor_msgs::msg::PointCloud>(
  //       "/pose_graph/match_points", 1000, std::bind(&Subscriber::relocCallback, this, std::placeholders::_1));
  // }

  tfBuffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
  imgTransport_ = 0;

  imgLeftCounter = 0;   // @Sharmin
  imgRightCounter = 0;  // @Sharmin
  // Added by Sharmin
  if (vioParameters_.histogramParams.histogramMethod == HistogramMethod::CLAHE) {
    clahe = cv::createCLAHE();
    clahe->setClipLimit(vioParameters_.histogramParams.claheClipLimit);
    clahe->setTilesGridSize(