沉默杀手HomeArchivesTagsCategories
结束轨迹
|激光SLAMCartographer源码解读|
Word count: 565|Reading time: 2 min

node_main.cc中的Run函数最后：

1
2
3
4
::ros::spin();
// 终止所有轨迹
node.FinishAllTrajectories();
node.RunFinalOptimization();

主要看FinishAllTrajectories

1
2
3
4
5
6
7
8
9
10
11
12
void Node::FinishAllTrajectories()
{
  carto::common::MutexLocker lock(&mutex_);
  for (auto& entry : is_active_trajectory_) {
    const int trajectory_id = entry.first;
    // 已经lock了，所以叫做 FinishTrajectoryUnderLock
    if (entry.second) {
      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
               cartographer_ros_msgs::StatusCode::OK);
    }
  }
}

offline_node.cc 中还调用了一个Node::FinishTrajectory(是这个函数唯一被调用的地方)，还有就是finish_trajectory服务的回调函数，以上3个函数实际都调用FinishTrajectoryUnderLock


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id)
{
  cartographer_ros_msgs::StatusResponse status_response;

  // First, check if we can actually finish the trajectory
  // trajectory_id 是否属于正在结束的轨迹集合，如果是 FROZEN 则返回
  if (map_builder_bridge_.GetFrozenTrajectoryIds().count(trajectory_id))
  {
    const std::string error =
        "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
    LOG(ERROR) << error;
    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    status_response.message = error;
    return status_response;
  }
  // trajectory_id 是否是 ACTIVE状态
  if (is_active_trajectory_.count(trajectory_id) == 0)
  {
    const std::string error =
        "Trajectory " + std::to_string(trajectory_id) + " is not created yet.";
    LOG(ERROR) << error;
    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
    status_response.message = error;
    return status_response;
  }
  // trajectory_id 是否已经 finish
  if (!is_active_trajectory_[trajectory_id])
  {
    const std::string error = "Trajectory " + std::to_string(trajectory_id) +
                              " has already been finished.";
    LOG(ERROR) << error;
    status_response.code =
        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
    status_response.message = error;
    return status_response;
  }
  // Shutdown the subscribers of this trajectory.
  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);
  CHECK(is_active_trajectory_.at(trajectory_id));
  // 主要是这里
  map_builder_bridge_.FinishTrajectory(trajectory_id);

  is_active_trajectory_[trajectory_id] = false;
  const std::string message =
      "Finished trajectory " + std::to_string(trajectory_id) + ".";
  status_response.code = cartographer_ros_msgs::StatusCode::OK;
  status_response.message = message;
  return status_response;
}

StatusResponse不正常的code是INVALID_ARGUMENT, NOT_FOUND, RESOURCE_EXHAUSTED, 只有OK是正常的

其中的关键函数就是：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
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_EQ(sensor_bridges_.count(trajectory_id), 1);
  map_builder_->FinishTrajectory(trajectory_id);
  sensor_bridges_.erase(trajectory_id);
}

// 再看这个函数
void MapBuilder::FinishTrajectory(const int trajectory_id) {
  sensor_collator_->FinishTrajectory(trajectory_id);
  pose_graph_->FinishTrajectory(trajectory_id);
}

sensor_collator_就是Collator::FinishTrajectory 或者 TrajectoryCollator::FinishTrajectory，不用关注。

重要的是后端的同名函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
void PoseGraph2D::FinishTrajectory(const int trajectory_id)
{
  AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_)
  {
    absl::MutexLock  locker(&mutex_);
    CHECK(!IsTrajectoryFinished(trajectory_id));   // 不能是已结束
    data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;

    for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
      data_.submap_data.at(submap.id).state = SubmapState::kFinished;
    }
    return WorkItem::Result::kRunOptimization;
  });
}

这里又是全局优化的标志kRunOptimization，然后又去PoseGraph2D::HandleWorkQueue，也是一次优化过程了