2D SLAM_Cartographer(2)_ビジネスロジック

11626 ワード
2D SLAM
cartographerはmain()関数を介してvoid Run()を呼び出してプログラムを起動し,Run()もcartographerのビジネスロジックを受け取った.
void Run() 
{
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  //tf2_ros::Buffer tf2 library 。Its main public API is defined by tf2_ros::BufferInterface. 
  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);
  
  auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(node_options.map_builder_options);
  Node node(node_options, std::move(map_builder), &tf_buffer,FLAGS_collect_metrics);
  
  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);
  }
  
  ::ros::spin();// ，  chatterCallback(),
  node.FinishAllTrajectories();
  node.RunFinalOptimization();

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

解析:
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer
  {
    ::ros::Duration(kTfBufferCacheTimeInSeconds)
  };
  tf2_ros::TransformListener tf(tf_buffer);//

//ここでは10 sのtfバッファを定義します.次に、cartographerシステムがパブリッシュしたすべての座標系変換の関係をリスニングし、tf_にキャッシュするリスナーtfを定義する.bufferでkTfBufferCacheTimeInSecondsはキャッシュ10 sを表す
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

NodeOptionsは/cartographer_ros/cartographer_ros/cartographer/node_options.hで定義する;プロファイルのロードに使用します.このstructには、tfを受信するtimeout時間設定、サブマップパブリッシュサイクル設定などの基本パラメータの設定が含まれています.tieは、LoadOptionsで取得したパラメータ値をnode_にそれぞれ割り当てます.optionsとtrajectory_options. LoadOptions関数はnode_options.hで定義します.
  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);
  }

LoadState()は、コマンドパラメータの形式で既存のシステムステータスファイルをロードすると、システムはこのステータスに従って実行できます.StartTrajectory WithDefaultTopics()は、デフォルトのtopicに従って、新しいトラックを起動し、nodeに入ります.ccはAddTrajectory(options,DefaultSensorTopics()を呼び出す.関数を使用して、各topicデータの受信を開始し、cartographer下位アルゴリズムを起動します.
 ::ros::spin();// ，  chatterCallback(),

シリーズの初期化と必要な変数定義が完了すると、論理ループが開き、nodeオブジェクトはメッセージコールバックマルチスレッドなどのメカニズムで位置決め構築プロセス全体を完了します.
  node.FinishAllTrajectories();
  node.RunFinalOptimization();

ROSの論理ループ中に終了メカニズムがトリガーされると,Run関数は以上の文を実行し,最終的な軌跡と地図の最適化を完了する.
  if (!FLAGS_save_state_filename.empty()) 
  {
    node.SerializeState(FLAGS_save_state_filename, true /* include_unfinished_submaps */);
  }

最後に、パラメータを実行して保存したシステムステータスファイルが空でない場合は、現在のシステムステータスをパラメータsave_に保存します.state_filenameで指定したファイルにあります.
はさみテスト
FormatMessageエラーメッセージ