Cartographer保存建图轨迹

需求：从cartographer建好的地图中保存建图轨迹。
思路：在使用cartographer建图的过程中，找到cartographer 提供的 “/trajectory_query” 服务（可以用“rosrun rqt_service_caller rqt_service_caller”命令），请求后会得到当前轨迹每一帧的位姿。在源码中找到这部分内容，将其保存为txt文件即可。
方法：源码node.cc中，修改HandleTrajectoryQuery函数：

bool Node::HandleTrajectoryQuery(
    ::cartographer_ros_msgs::TrajectoryQuery::Request& request,
    ::cartographer_ros_msgs::TrajectoryQuery::Response& response) {
  absl::MutexLock lock(&mutex_);
  response.status = TrajectoryStateToStatus(
      request.trajectory_id,
      {TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
       TrajectoryState::FROZEN} /* valid states */);
  if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
    LOG(ERROR) << "Can't query trajectory from pose graph: "
               << response.status.message;
    return true;
  }
  map_builder_bridge_.HandleTrajectoryQuery(request, response);

  // 保存轨迹为txt文件
  std::cout << "receiving" << std::endl;
  std::string txt_filename = "/home/xxx/xxx.txt";
  std::ofstream outf(txt_filename, std::ios::app);
  for (int i = 0; i < response.trajectory.size(); i++){
    int32_t sec = response.trajectory[i].header.stamp.sec;
    int32_t nsec = response.trajectory[i].header.stamp.nsec;
    double x = response.trajectory[i].pose.position.x;
    double y = response.trajectory[i].pose.position.y;
    double z = response.trajectory[i].pose.position.z;
    outf << std::setprecision(20) << sec << "." << nsec << " " << x << " " << y << " " << z << std::endl;
  }

  return true;
}

效果：
轨迹:

点云+轨迹: