Cartographer学习之node_main.cc II

就不告诉你噢

已于 2023-03-16 11:16:22 修改

阅读量217

点赞数 1

文章标签：学习

于 2023-03-16 11:11:50 首次发布

本文链接：https://blog.csdn.net/m0_55879913/article/details/129560496

版权

这节继续学习主函数，主要介绍其中的Run函数部分

  // 这里定义了tf发布的时间间隔 
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  // 实例化tf2_ros::Buffer对象tf_buffer
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  // 开启监听tf的独立线程
  tf2_ros::TransformListener tf(tf_buffer);

1.NodeOptions结构体

接着分别介绍了两种结构体，先转到NodeOptions结构体（node_options.h）定义处，成员包含地图帧名、接收tf的timeout时间、子图发布周期、位姿发布周期、轨迹发布周期等。

struct NodeOptions {
  ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
  std::string map_frame;
  double lookup_transform_timeout_sec;
  double submap_publish_period_sec;
  double pose_publish_period_sec;
  double trajectory_publish_period_sec;
  bool publish_to_tf = true;
  bool publish_tracked_pose = false;
  bool use_pose_extrapolator = true;
};

2.TrajectoryOptions结构体

然后是TrajectoryOptions（trajectory_options.h），这个结构体成员主要与轨迹信息发布有关：是否使用odom里程计，gps，landmark或者配置单线/多线激光雷达数据，超声波雷达，点云数据等。

struct TrajectoryOptions {
  ::cartographer::mapping::proto::TrajectoryBuilderOptions
      trajectory_builder_options;
  std::string tracking_frame;
  std::string published_frame;
  std::string odom_frame;
  bool provide_odom_frame;
  bool use_odometry;
  bool use_nav_sat;
  bool use_landmarks;
  bool publish_frame_projected_to_2d;
  int num_laser_scans;
  int num_multi_echo_laser_scans;
  int num_subdivisions_per_laser_scan;
  int num_point_clouds;
  double rangefinder_sampling_ratio;
  double odometry_sampling_ratio;
  double fixed_frame_pose_sampling_ratio;
  double imu_sampling_ratio;
  double landmarks_sampling_ratio;
};

3.LoadOptions函数

使用std::tie()函数，根据Lua文件中的配置文件信息给node_options以及trajectory_options进行赋值。一般该函数通过引用方式将元素绑定到tuple上，并可以进行解压tuple/pair。

std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

转到LoadOptions(node_options.cc)定义处，其中std::tuple是一个元组数据类型，可以将不同类型的元素封装在一起(可以简单的理解为结构体变量)。

std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
    const std::string& configuration_directory,
    const std::string& configuration_basename) {
  // 获取配置文件所在的目录
  auto file_resolver =
      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
          std::vector<std::string>{configuration_directory});
        
  // 读取配置文件内容到code中
  const std::string code =
      file_resolver->GetFileContentOrDie(configuration_basename);

  // 根据给定的字符串, 生成一个lua字典
  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
      code, std::move(file_resolver));

  // 创建元组tuple,元组定义了一个有固定数目元素的容器, 其中的每个元素类型都可以不相同
  // 将配置文件的内容填充进NodeOptions与TrajectoryOptions, 并返回
  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
                         CreateTrajectoryOptions(&lua_parameter_dictionary));
}

上述代码中std::move是将对象的状态或者所有权从一个对象转移到另一个对象，只涉及转移，不需要进行内容的迁移以及内存的拷贝，能提升效率。右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象,这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.而GetFileContentOrDie函数用来检测某个文件内容是否为空，转到定义处（configuration_file_resolver.cc)。

std::string ConfigurationFileResolver::GetFileContentOrDie(
    const std::string& basename) {
  // CHECK进行检测，empty（）函数检测文件内容是否为空，非空继续运行否则打印后面警告
  CHECK(!basename.empty()) << "File basename cannot be empty." << basename;

  // 根据文件名查找是否在给定文件夹中存在
  const std::string filename = GetFullPathOrDie(basename);
  std::ifstream stream(filename.c_str());

  // 读取配置文件内容并返回
  return std::string((std::istreambuf_iterator<char>(stream)),
                     std::istreambuf_iterator<char>());
}

4.Map_Builder类

回到Run（）函数，map_builder是完整的SLAM算法类，包含前端(TrajectoryBuilders,scan to submap) 与后端(用于查找回环的PoseGraph)。可以进入map_builder.h进行查看。

auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);

class MapBuilder : public MapBuilderInterface {
 public:
  explicit MapBuilder(const proto::MapBuilderOptions &options);
  ~MapBuilder() override {}

  MapBuilder(const MapBuilder &) = delete;
  MapBuilder &operator=(const MapBuilder &) = delete;

  int AddTrajectoryBuilder(
      const std::set<SensorId> &expected_sensor_ids,
      const proto::TrajectoryBuilderOptions &trajectory_options,
      LocalSlamResultCallback local_slam_result_callback) override;

  int AddTrajectoryForDeserialization(
      const proto::TrajectoryBuilderOptionsWithSensorIds
          &options_with_sensor_ids_proto) override;

  void FinishTrajectory(int trajectory_id) override;

  std::string SubmapToProto(const SubmapId &submap_id,
                            proto::SubmapQuery::Response *response) override;

  void SerializeState(bool include_unfinished_submaps,
                      io::ProtoStreamWriterInterface *writer) override;

  bool SerializeStateToFile(bool include_unfinished_submaps,
                            const std::string &filename) override;

  std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader,
                               bool load_frozen_state) override;

  std::map<int, int> LoadStateFromFile(const std::string &filename,
                                       const bool load_frozen_state) override;

  mapping::PoseGraphInterface *pose_graph() override {
    return pose_graph_.get();
  }

  int num_trajectory_builders() const override {
    return trajectory_builders_.size();
  }

  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

  const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
      &GetAllTrajectoryBuilderOptions() const override {
    return all_trajectory_builder_options_;
  }

 private:
  const proto::MapBuilderOptions options_;
  common::ThreadPool thread_pool_;

  std::unique_ptr<PoseGraph> pose_graph_;

  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
  std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
      trajectory_builders_;
  std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
      all_trajectory_builder_options_;
};

接着进行Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder。

  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

5.LoadState函数

检测load_state_filename文件，如果非空则加载执行pbstream函数。

if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

进入LoadState(node.cc)定义处，第一个参数为加载数据包名称，第二个参数为是否加载已保存的非优化路径状态,再加上一把互斥锁，防止多个对象同时访问该资源产生冲突。

void Node::LoadState(const std::string& state_filename,
                     const bool load_frozen_state) {
  absl::MutexLock lock(&mutex_);
  map_builder_bridge_.LoadState(state_filename, load_frozen_state);
  load_state_ = true;
}

6.StartTrajectoryWithDefaultTopics函数

同理当start_trajectory_with_default_topics为true，以默认话题进行轨迹的更新。

 // 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

接着进入StartTrajectoryWithDefaultTopics(node.cc)函数详细看看。

// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

这块调用了ValidateTrajectoryOptions这个函数，主要用来判断2d以及3d的配置信息。如果检测到是2d的则返回相应2d的建图选项信息，3d同理。

bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_2d_options();
  }
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_3d_options();
  }
  return false;
}

再根据配置信息来添加一条轨迹。

::ros::spin()

7.FinishAllTrajectories函数

  // 结束所有处于活动状态的轨迹
  node.FinishAllTrajectories();

进入定义处(node.cc)

void Node::FinishAllTrajectories() {
  absl::MutexLock lock(&mutex_);
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
    if (entry.second == TrajectoryState::ACTIVE) {
      const int trajectory_id = entry.first;
      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
               cartographer_ros_msgs::StatusCode::OK);
    }
  }
}

这里分别调用了MapBuilderBridge类中的GetTrajectoryStates函数和node类内的FinishTrajectoryUnderLock函数。先来分析前者：

std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
  auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();
  // Add active trajectories that are not yet in the pose graph, but are e.g.
  // waiting for input sensor data and thus already have a sensor bridge.
  for (const auto& sensor_bridge : sensor_bridges_) {
    trajectory_states.insert(std::make_pair(
        sensor_bridge.first,
        ::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE));
  }
  return trajectory_states;
}

向全局优化(pose graph)中添加新的active状态的轨迹，并返回所有的状态轨迹。然后为活跃的轨迹添加active状态, 如果trajectory_states中存在这个轨迹id,则会被忽略不会被添加进去。返回的是存储轨迹状态的map容器。

判断entry.second是否为active状态，最后用CHECK_EQ宏来检测trajectory_id).code的状态，并调用FinishTrajectoryUnderLock函数：

cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id) {
  cartographer_ros_msgs::StatusResponse status_response;
  // Step: 1 检查 trajectory_id 是否在 正在结束的轨迹集合中
  if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
    status_response.message = absl::StrCat("Trajectory ", trajectory_id,
                                           " already pending to finish.");
    status_response.code = cartographer_ros_msgs::StatusCode::OK;
    LOG(INFO) << status_response.message;
    return status_response;
  }

  // First, check if we can actually finish the trajectory.
  // Step: 2 检查这个轨迹是否存在, 如果存在则检查这个轨迹是否是ACTIVE状态
  status_response = TrajectoryStateToStatus(
      trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
  // 如果不是OK状态就返回ERROR
  if (status_response.code != cartographer_ros_msgs::StatusCode::OK) {
    LOG(ERROR) << "Can't finish trajectory: " << status_response.message;
    return status_response;
  }

  // Shutdown the subscribers of this trajectory.
  // A valid case with no subscribers is e.g. if we just visualize states.
  // Step: 3 如果这个轨迹存在subscribers, 则先关闭subscriber
  if (subscribers_.count(trajectory_id)) {
    for (auto& entry : subscribers_[trajectory_id]) {
      entry.subscriber.shutdown();
      subscribed_topics_.erase(entry.topic);
      LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
    }
    // 在subscribers_中将这条轨迹的信息删除
    CHECK_EQ(subscribers_.erase(trajectory_id), 1);
  }

  // Step: 4 调用cartographer中的map_builder的FinishTrajectory()进行轨迹的结束
  map_builder_bridge_.FinishTrajectory(trajectory_id);
  // 将这个轨迹id放进正在结束的轨迹集合中
  trajectories_scheduled_for_finish_.emplace(trajectory_id);
  status_response.message =
      absl::StrCat("Finished trajectory ", trajectory_id, ".");
  status_response.code = cartographer_ros_msgs::StatusCode::OK;
  return status_response;
}

8.RunFinalOptimization函数

当所有轨迹结束时，再进行一次全局优化

node.RunFinalOptimization();

转到定义处：

void Node::RunFinalOptimization() {
  {
    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
      const int trajectory_id = entry.first;
      if (entry.second == TrajectoryState::ACTIVE) {
        LOG(WARNING)
            << "Can't run final optimization if there are one or more active "
               "trajectories. Trying to finish trajectory with ID "
            << std::to_string(trajectory_id) << " now.";
        CHECK(FinishTrajectory(trajectory_id))
            << "Failed to finish trajectory with ID "
            << std::to_string(trajectory_id) << ".";
      }
    }
  }
  // Assuming we are not adding new data anymore, the final optimization
  // can be performed without holding the mutex.
  map_builder_bridge_.RunFinalOptimization();
}

最后如果save_state_filename非空, 就保存pbstream文件

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