Cartographer学习记录：cartographer源码3D SLAM部分（三）

最新推荐文章于 2024-07-15 23:36:33 发布

不甘平凡的平凡之人

最新推荐文章于 2024-07-15 23:36:33 发布

阅读量616

点赞数

文章标签：学习 3d 自动驾驶

本文链接：https://blog.csdn.net/smartfoolish/article/details/123215545

版权

上一篇是由于node_main.cc中调用了node类的构造函数，所以展开阅读了下node.h和node.cc文件，从而大致了解node类内各种函数的主要功能。本篇将回到node_main.cc文件，继续读完剩余的代码。从node类的构造函数往下阅读，并了解如何具体应用node类的成员函数。

1.LoadState函数

if (!FLAGS_load_state_filename.empty()) {
    //加载数据包数据;
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

在node_main.cc函数的前面中，若是load_state_filename设置为非空，则调用node类的LoadState函数加载数据包。如下是具体实现：

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);
}

可以看到调用了MapBuilderBridge类的LoadState函数，跟着深入到MapBuilderBridge类，在map_builder_bridge.h中可以查看到其声明为：

void LoadState(const std::string& state_filename, bool load_frozen_state);

参数一为加载数据包名称，参数二为是否加载已保存的非优化路径状态。

接下来转到map_builder_bridge.cc文件中查看具体实现：

void MapBuilderBridge::LoadState(const std::string& state_filename,
                                 bool load_frozen_state) {
  // Check if suffix of the state file is ".pbstream".
  const std::string suffix = ".pbstream";
  CHECK_EQ(state_filename.substr(
               std::max<int>(state_filename.size() - suffix.size(), 0)),
           suffix)
      << "The file containing the state to be loaded must be a "
         ".pbstream file.";
  LOG(INFO) << "Loading saved state '" << state_filename << "'...";
  cartographer::io::ProtoStreamReader stream(state_filename);
  map_builder_->LoadState(&stream, load_frozen_state);
}

从这里我们可以发现，MapBuilderBridge类的LoadState函数调用了 MapBuilder的成员函数 LoadState 来加载一个pbstream 文件，所以具体细节问题留到后续更新的cartographer底层代码阅读时讲解。map_builder_是接口MapBuilderInterface 的实例化对象,而根据是 2d 还是 3d 情况,其具体实现会略有不同，至于如何具体化为2D或者3D对象，在后面我也会慢慢细说。

2.StartTrajectoryWithDefaultTopics函数

if (FLAGS_start_trajectory_with_default_topics) {
    //以默认话题开始规划路径;
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

同理，在node_main.cc函数的前面也是定义了start_trajectory_with_default_topics，默认情况下为true，cartographer的ROS上层也是从这里开始以默认话题进行轨迹的更新，这里稍带提一句，默认的话题名称设置可以到node_constants.h里面进行设置，如下所示：

// Default topic names; expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan";
constexpr char kMultiEchoLaserScanTopic[] = "echoes";
constexpr char kPointCloud2Topic[] = "points2";
constexpr char kImuTopic[] = "imu";
constexpr char kOdometryTopic[] = "odom";
constexpr char kNavSatFixTopic[] = "fix";
constexpr char kLandmarkTopic[] = "landmark";
constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
constexpr char kOccupancyGridTopic[] = "map";
constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kTrackedPoseTopic[] = "tracked_pose";
constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kTrajectoryQueryServiceName[] = "trajectory_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteStateServiceName[] = "write_state";
constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
constexpr char kReadMetricsServiceName[] = "read_metrics";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
constexpr char kConstraintListTopic[] = "constraint_list";
constexpr double kConstraintPublishPeriodSec = 0.5;
constexpr double kTopicMismatchCheckDelaySec = 3.0;

constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;

下面回到node.h里面有关StartTrajectoryWithDefaultTopics函数的声明：

void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);

参数只有一个，是有关轨迹的配置信息，也就是在trajectory_builder.lua文件里面的配置。下面看node.cc文件内的具体实现：

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  CHECK(ValidateTrajectoryOptions(options));
  AddTrajectory(options);
}

可以看到简单三行代码，先是设置互斥锁，防止多个对象同时访问该资源产生冲突，然后是调用node类的成员函数ValidateTrajectoryOptions对配置信息进行验证，根据2D和3D的不同进行是否拥有相关设置信息的验证，否则返回false，最后是根据配置信息添加轨迹信息，下面为两个函数的具体实现。

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;
}

从node类的AddTrajectory函数我们可以看到主要调用了ComputeExpectedSensorIds、MapBuilderBridge类的AddTrajectory函数、AddExtrapolator、AddSensorSamplers、LaunchSubscribers函数，这里将继续挖掘MapBuilderBridge类的AddTrajectory函数，其余函数的功能可以参照上一篇，主要是返回具体topic名称、添加位姿估计器、添加传感器采样器、ROS topic订阅与回调函数。

int Node::AddTrajectory(const TrajectoryOptions& options) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddExtrapolator(trajectory_id, options);
  AddSensorSamplers(trajectory_id, options);
  LaunchSubscribers(options, trajectory_id);
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

同样先看map_builder_bridge.h文件内的声明：

int AddTrajectory(
      const std::set<
          ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
          expected_sensor_ids,
      const TrajectoryOptions& trajectory_options);

输入参数为指定的topic名称集合、轨迹配置信息，下面看map_builder_bridge.cc文件内的具体实现：

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  //调用了 map_builder_->AddTrajectoryBuilder，传入的数据有传感器的 id,trajectory 的一些配置参数,以及一个lambda表达式    
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      [this](const int trajectory_id, const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id));
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

可以清楚的看到，依旧是稳健地调用了MapBuilder类的AddTrajectoryBuilder函数，其余主要是打印错误信息以及检查相应参数变量是否正确，该函数返回的是添加的轨迹编号。

3.FinishAllTrajectories函数

node.h文件内的声明：

void 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函数。

（1）MapBuilderBridge类的GetTrajectoryStates函数依旧与MapBuilder类中的posegraph（用于全局优化）的GetTrajectoryStates函数相关，返回的是存储轨迹状态的map容器。

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;
}

（2）FinishTrajectoryUnderLock函数前面检查了一下是否可以关掉,指定 id 是否存在,是否已经被 Finished 了等情况后,如果一切正常,则停止订阅 Topic、清除 id 及其他与该 trajectory 相关的量。最后调用 map_builder_bridge_中的FinishTrajectory 函数。

cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id) {
  cartographer_ros_msgs::StatusResponse status_response;
  //对是否可以结束相应路径进行判断，并进行异常处理
  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.
  status_response = TrajectoryStateToStatus(
      trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
  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.
  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 << "]";
    }
    CHECK_EQ(subscribers_.erase(trajectory_id), 1);
  }
  map_builder_bridge_.FinishTrajectory(trajectory_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;
}

map_builder_bridge_中的FinishTrajectory 函数主要细节藏在MapBuilder类的FinishTrajectory函数。

void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";

  // Make sure there is a trajectory with 'trajectory_id'.
  CHECK(GetTrajectoryStates().count(trajectory_id));
  map_builder_->FinishTrajectory(trajectory_id);
  sensor_bridges_.erase(trajectory_id);
}

到此，cartographer的上层基本已经介绍完毕，俗话说的好，“魔鬼总是藏在细节中”，通过阅读我们发现MapBuilderBridge这个类很形象，完美地承担了“桥梁”的作用，搭建了我们从上层通向底层算法的路，接下来，我将仔细解读MapBuilder这个类是如何一步一步完成我们最终的SLAM的。