Method Subscriber

pose_graph/src/pose_graph/Subscriber.cpp:13–71

#include "utils/UtilsOpenCV.h"

Subscriber::Subscriber(std::shared_ptr<rclcpp::Node> node, Parameters& params) : node_(node), params_(params) {
  // TODO(bjoshi): pass as params from roslaunch file
  kf_image_topic_ = "/keyframe_imageL";
  kf_pose_topic_ = "/keyframe_pose";
  kf_points_topic_ = "/keyframe_points";
  svin_reloc_odom_topic_ = "/relocalization_odometry";
  svin_health_topic_ = "/svin_health";
  primitive_estimator_topic_ = "/aqua_primitive_estimator/odometry";
  last_image_time_ = -1;
  raw_image_topic_ = "/cam0/image_raw";

  rclcpp::QoS qos(10);
  auto rmw_qos_profile = qos.get_rmw_qos_profile();

  keyframe_image_subscriber_.subscribe(node_, kf_image_topic_, rmw_qos_profile);
  keyframe_points_subscriber_.subscribe(node_, kf_points_topic_, rmw_qos_profile);
  keyframe_pose_subscriber_.subscribe(node_, kf_pose_topic_, rmw_qos_profile);
  svin_health_subscriber_.subscribe(node_, svin_health_topic_, rmw_qos_profile);

  static constexpr size_t kMaxKeyframeSynchronizerQueueSize = 10u;
  sync_keyframe_ = std::make_unique<message_filters::Synchronizer<keyframe_sync_policy>>(
      keyframe_sync_policy(kMaxKeyframeSynchronizerQueueSize),
      keyframe_image_subscriber_,
      keyframe_pose_subscriber_,
      keyframe_points_subscriber_,
      svin_health_subscriber_);
  sync_keyframe_->registerCallback(std::bind(&Subscriber::keyframeCallback,
                                             this,
                                             std::placeholders::_1,
                                             std::placeholders::_2,
                                             std::placeholders::_3,
                                             std::placeholders::_4));

  if (params_.global_mapping_params_.enabled) {
    sub_orig_image_ = node_->create_subscription<sensor_msgs::msg::Image>(
        raw_image_topic_, 100, std::bind(&Subscriber::imageCallback, this, std::placeholders::_1));
  }
  if (params_.health_params_.enabled) {
    sub_primitive_estimator_ = node_->create_subscription<nav_msgs::msg::Odometry>(
        primitive_estimator_topic_,
        100,
        std::bind(&Subscriber::primitiveEstimatorCallback, this, std::placeholders::_1));
  }

  // Watchdog: parameter and timer setup (uses steady clock)
  try {
    if (!node_->has_parameter("freeze_timeout_sec")) {
      node_->declare_parameter<double>("freeze_timeout_sec", 60.0);
    }
    node_->get_parameter("freeze_timeout_sec", freeze_timeout_sec_);
  } catch (const std::exception&) {
    freeze_timeout_sec_ = 240.0;
  }
  last_keyframe_tp_ = std::chrono::steady_clock::now();
  frozen_ = false;
  seen_first_keyframe_ = false;
  using namespace std::chrono_literals;  // NOLINT
  watchdog_timer_ = node_->create_wall_timer(500ms, std::bind(&Subscriber::watchdogTick, this));