Function main

pose_graph/src/pose_graph_node.cpp:97–256

}

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  options.allow_undeclared_parameters(true);
  options.automatically_declare_parameters_from_overrides(true);

  auto node = std::make_shared<rclcpp::Node>("pose_graph_node");

  // Initialize Google's logging library.
  google::InitGoogleLogging(argv[0]);

  FLAGS_stderrthreshold = 0;  // INFO: 0, WARNING: 1, ERROR: 2, FATAL: 3
  FLAGS_colorlogtostderr = 1;

  // read parameters
  std::string config_file;

  node->declare_parameter<std::string>("config_file", "");
  node->get_parameter<std::string>("config_file", config_file);

  if (config_file.empty()) {
    LOG(ERROR) << "Config file not provided";
    return EXIT_FAILURE;
  }

  Parameters params;
  params.loadParameters(config_file);

  if (params.debug_mode_) {
    setupOutputLogDirectories(params.debug_output_path_);
    FLAGS_v = 0;
  }

  auto subscriber = std::make_unique<Subscriber>(node, params);
  auto loop_closure = std::make_unique<LoopClosure>(params);
  auto publisher = std::make_unique<Publisher>(node, params.debug_mode_);

  loop_closure->setKeyframePoseCallback(
      std::bind(&Publisher::publishKeyframePath, publisher.get(), std::placeholders::_1, std::placeholders::_2));
  loop_closure->setLoopClosureCallback(
      std::bind(&Publisher::publishLoopClosurePath, publisher.get(), std::placeholders::_1, std::placeholders::_2));

  if (params.debug_mode_) {
    loop_closure->setPrimitivePublishCallback(
        std::bind(&Publisher::publishPrimitiveEstimator, publisher.get(), std::placeholders::_1));
  }

  subscriber->registerKeyframeCallback(
      std::bind(&LoopClosure::fillKeyframeTrackingQueue, loop_closure.get(), std::placeholders::_1));

  if (params.health_params_.enabled) {
    subscriber->registerPrimitiveEstimatorCallback(
        std::bind(&LoopClosure::fillPrimitiveEstimatorBuffer, loop_closure.get(), std::placeholders::_1));
  }

  rclcpp::TimerBase::SharedPtr timer;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pointcloud_service;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr save_trajectory_service;