结束轨迹-Node类-cartographer
Kong Liangqian Lv6
 2021-09-04 15:59:22  

在node_main.cc中，当::ros::spin()结束了之后（在运行node的时候，按下Ctrl+C即结束spin开始运行spin之后的语句），开始结束所有处于活动状态的轨迹，

1
2
3
4
5
6
7
8
void Run() {
  ...
  ::ros::spin();
  // 结束所有处于活动状态的轨迹
  node.FinishAllTrajectories();
  ...
}

下面是FinishAllTrajectories的详细代码。

1
2
3
4
5
6
7
8
9
10
11
// 结束所有处于活动状态的轨迹
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);
    }
  }
}

此函数，调用map_builder_bridge来获取轨迹的状态，key为轨迹ID，value为轨迹状态，轨迹状态一共有四种

1
enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };

如果有轨迹的状态为ACTIVE，那么我们就通过FinishTrajectoryUnderLock, 将其进行结束操作

FinishTrajectoryUnderLock

此函数用于结束一条指定id的轨迹，返回一个状态，用于判定是否结束成功

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
/**
 * @brief 结束指定id的轨迹
 *
 * @param[in] trajectory_id 要结束的轨迹的id
 * @return cartographer_ros_msgs::StatusResponse
 */
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;
}

检查一：是否在trajectoriesscheduled_for_finish里面

如果判定轨迹已经在将要结束的轨迹中，trajectoriesscheduled_for_finish，则直接给status_response一个OK状态

1
2
3
4
5
6
7
8
// 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;
}

检查二：检查轨迹是否存在和状态

检查这个轨迹是否存在, 如果存在则检查这个轨迹是否是ACTIVE状态

1
2
3
4
5
6
7
8
9
// 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;
}

检查三：若存在，检查subscriber是否关闭

如果通过了步骤二，说明轨迹存在并且状态是ACTIVE。那么开始检查对应的轨迹ID是否有订阅者，使用count可以查看map里面的对应key的value个数，使用shutdown来关闭对应的订阅者。并且移除记录subscribedtopics 里面记录的已经有的topic名字。最后把subscribers_ 里面的轨迹ID给清除掉

1
2
3
4
5
6
7
8
9
10
// 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);
}

结束轨迹

调用cartographer中的mapbuilder的FinishTrajectory()进行轨迹的结束，然后放入trajectories_scheduled_for_finish，设置status_response的message和code即可

1
2
3
4
5
6
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;
 Comments