Method StereoSync

okvis_ros/src/stereo_sync.cpp:22–68

class StereoSync : public rclcpp::Node {
 public:
  explicit StereoSync(const rclcpp::NodeOptions& options) : Node("stereo_sync", options) {
    previous_stamp_ = rclcpp::Time(0, 0);

    this->get_parameter("left_img_topic", left_img_topic_);
    this->get_parameter("right_img_topic", right_img_topic_);
    this->get_parameter("compressed", compressed_image_);
    this->get_parameter("config_filename", config_file_);

    cv::FileStorage fsSettings(config_file_, cv::FileStorage::READ);

    if (!fsSettings.isOpened()) {
      RCLCPP_FATAL_STREAM(this->get_logger(), "ERROR: Wrong path to settings\n");
    }

    float frame_rate = static_cast<float>(fsSettings["camera_params"]["camera_rate"]);
    float max_time_diff_between_msgs = 0.5f / frame_rate;
    RCLCPP_INFO_STREAM(this->get_logger(), "Setting Age Penalty to:  " << max_time_diff_between_msgs);
    rclcpp::QoS qos(10);
    auto rmw_qos_profile = qos.get_rmw_qos_profile();
    static constexpr size_t kImageSynchronizerQueueSize = 10u;

    if (compressed_image_) {
      left_compressed_img_topic_ = left_img_topic_ + "/compressed";
      right_compressed_img_topic_ = right_img_topic_ + "/compressed";

      left_compressed_img_sub_.subscribe(this, left_compressed_img_topic_, rmw_qos_profile);
      right_compressed_img_sub_.subscribe(this, right_compressed_img_topic_, rmw_qos_profile);
      compressed_image_synchronizer_ = std::make_unique<message_filters::Synchronizer<compressed_image_sync_policy>>(
          compressed_image_sync_policy(kImageSynchronizerQueueSize),
          left_compressed_img_sub_,
          right_compressed_img_sub_);
      compressed_image_synchronizer_->registerCallback(
          std::bind(&StereoSync::compressedImageCallback, this, std::placeholders::_1, std::placeholders::_2));
      compressed_image_synchronizer_->setAgePenalty(max_time_diff_between_msgs);
    } else {
      left_img_sub_.subscribe(this, left_img_topic_, rmw_qos_profile);
      right_img_sub_.subscribe(this, right_img_topic_, rmw_qos_profile);
      image_synchonizer_ = std::make_unique<message_filters::Synchronizer<image_sync_policy>>(
          image_sync_policy(kImageSynchronizerQueueSize), left_img_sub_, right_img_sub_);
      image_synchonizer_->registerCallback(
          std::bind(&StereoSync::imageCallback, this, std::placeholders::_1, std::placeholders::_2));
      image_synchonizer_->setAgePenalty(max_time_diff_between_msgs);
    }

    left_img_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/cam0/image_raw", 100);
    right_img_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/cam1/image_raw", 100);
  }

 private:
  std::string left_img_topic_{}, right_img_topic_{};