cartographer_ros node （三）：结束轨迹

上一次我们详细讲述了node_main中对应的开始轨迹的函数StartTrajectoryWithDefaultTopics，这次我们主要讲解在结束SLAM之后调用的node类中的两个函数FinishAllTrajectories和RunFinalOptimization()
首先是FinishAllTrajectories，这个函数用于结束所有处于活动状态的轨迹

// 结束所有处于活动状态的轨迹
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);
    }
  }
}

其中主要调用了FinishTrajectoryUnderLock(trajectory_id)来结束掉对应的轨迹，这个函数中主要作了以下几件事：

• 检查 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;
  }
  

• 检查这个轨迹是否存在, 如果存在则检查这个轨迹是否是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;
  }
  

  其中的TrajectoryStateToStatus主要检查了轨迹id是否存在以及对应轨迹id的轨迹的状态是否为ACTIVE

  	cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus(
      const int trajectory_id, const std::set<TrajectoryState>& valid_states) {
    const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
    cartographer_ros_msgs::StatusResponse status_response;
  
    const auto it = trajectory_states.find(trajectory_id);
    // 如果没有找到对应id的轨迹, 返回NOT_FOUND
    if (it == trajectory_states.end()) {
      status_response.message =
          absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
      status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
      return status_response;
    }
  
    status_response.message =
        absl::StrCat("Trajectory ", trajectory_id, " is in '",
                     TrajectoryStateToString(it->second), "' state.");
    // 轨迹的状态如果在列表中, 返回OK, 否则返回 INVALID_ARGUMENT
    // 注意这里的列表中只有一个元素就是ACTIVE
    status_response.code =
        valid_states.count(it->second)
            ? cartographer_ros_msgs::StatusCode::OK
            : cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    return status_response;
  }
  

• 如果这个轨迹存在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);
  }
  

• 调用map_builder中的的FinishTrajectory()进行轨迹的结束并将轨迹id加入正在结束的轨迹集合中

    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;
  

其次是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) << ".";
      }
    }
  }
  // 这里假设我们已经不再接收新的数据了，所以这里进行全局优化时不需要上琐.
  map_builder_bridge_.RunFinalOptimization();
}