代码修改记录-cartographer_ros Run函数

Posted 2022-07-13 COCO_PEAK_NOODLE

代码：

void Run() 
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer::ros::Duration(kTfBufferCacheTimeInSeconds);
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  
  TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =
       LoadOptions(FLAGS_configuration_directory,
                   FLAGS_configuration_basename_server);
  
  //slam 读取slam建图的参数
  cartographer::cloud::proto::MapBuilderServerOptions
      map_builder_slam_options =
          cartographer::cloud::LoadMapBuilderServerOptions(
              FLAGS_configuration_directory,
              FLAGS_configuration_slam_basename);

  //navigation 读取navigation导航的参数
  cartographer::cloud::proto::MapBuilderServerOptions
      map_builder_navigation_options =
          cartographer::cloud::LoadMapBuilderServerOptions(
              FLAGS_configuration_directory,
              FLAGS_configuration_navigation_basename);

  // map_builder 为共享指针类型，这样既可以给map_builder_server使用，
  // 也可以给node使用，map_builder_server可以用来使用grpc调用接口
  auto map_builder =
      std::make_shared<cartographer::mapping::MapBuilder>(  // okagv default is absl::make_unique
         const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(
           map_builder_slam_options.map_builder_options()));

  std::unique_ptr<cartographer::cloud::MapBuilderServerInterface>
      map_builder_server =
          absl::make_unique<cartographer::cloud::MapBuilderServer>(
              map_builder_slam_options, map_builder);

  map_builder_server->Start();
  
  Node node(node_options, map_builder, &tf_buffer, FLAGS_collect_metrics);

  //把配置文件名及配置信息传给node
  if (!FLAGS_configuration_directory.empty()) 
    node.SetConfigurationParam(FLAGS_configuration_directory,
                               FLAGS_configuration_slam_basename,
                               FLAGS_configuration_navigation_basename);

    node.RecordMapBuilderOption(map_builder_slam_options,
                                map_builder_navigation_options);
  
  
  if (!FLAGS_load_state_filename.empty()) 
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  
  
  if (FLAGS_start_trajectory_with_default_topics) 
     //node.StartTrajectoryWithDefaultTopics(trajectory_options);
       node.StartTrajectoryWithNavigation(FLAGS_load_state_filename);
  
  
  LOG(WARNING) << "SLAM and Navigation System Start";
  ::ros::spin();
  map_builder_server->Shutdown();
  node.FinishAllTrajectories();
  node.RunFinalOptimization();

  if (!FLAGS_save_state_filename.empty()) 
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);