1-首先是结束建图路径

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)) {
      //LOG(INFO) << "Peak.ding data_.submap_data 15";
      data_.submap_data.at(submap.id).state = SubmapState::kFinished;
    }
    return WorkItem::Result::kRunOptimization;
  });
  */
  
    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)) {
      //LOG(INFO) << "Peak.ding data_.submap_data 15";
      data_.submap_data.at(submap.id).state = SubmapState::kFinished;
    }
    
}

2-然后是删除路径

void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
  {
    absl::MutexLock locker(&mutex_);

    auto it = data_.trajectories_state.find(trajectory_id);
    if (it == data_.trajectories_state.end()) {
      LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
                   << trajectory_id;
      return;
    }
    it->second.deletion_state =
        InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
  }

  /*
  AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    CHECK(data_.trajectories_state.at(trajectory_id).state !=
          TrajectoryState::ACTIVE);
    CHECK(data_.trajectories_state.at(trajectory_id).state !=
          TrajectoryState::DELETED);
    CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
          InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION);
    data_.trajectories_state.at(trajectory_id).deletion_state =
        InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;
    //return WorkItem::Result::kDoNotRunOptimization;
    return WorkItem::Result::kRunOptimization; //Peak.ding
  });
  */

  // okagv
  
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::ACTIVE);
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::DELETED);
  CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
        InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION);
  data_.trajectories_state.at(trajectory_id).deletion_state =
      InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;

  DeleteTrajectoriesIfNeeded();
  
}

3-注释原来DeleteTrajectoriesIfNeeded（）执行的位置，会有bug，相应修改即可。