cartographer_thread_pool

古路

已于 2022-10-19 11:15:45 修改

阅读量903

点赞数 1

分类专栏： # 2DLidar # cartographer 文章标签： cartographer线程池 cartographer thread pool cartographer线程线程池

于 2022-10-10 10:25:22 首次发布

本文链接：https://blog.csdn.net/fb_941219/article/details/127233122

版权

2DLidar 同时被 2 个专栏收录

21 篇文章 2 订阅

订阅专栏

cartographer

12 篇文章 2 订阅

订阅专栏

0.基础知识

请添加图片描述

理解参考

线程池看源代码更好理解，这里就不贴代码了。看看测试用例，了解使用过程。

1.从test文件理解Thread_Pool使用逻辑

1.1.task_test

TEST_F(TaskTest, RunTaskWithTwoDependency) {
  /*         c \
   *  a -->  b --> d
   */
  auto a = absl::make_unique<Task>();
  auto b = absl::make_unique<Task>();
  auto c = absl::make_unique<Task>();
  auto d = absl::make_unique<Task>();
  MockCallback callback_a;
  a->SetWorkItem([&callback_a]() { callback_a.Run(); });
  MockCallback callback_b;
  b->SetWorkItem([&callback_b]() { callback_b.Run(); });
  MockCallback callback_c;
  c->SetWorkItem([&callback_c]() { callback_c.Run(); });
  MockCallback callback_d;
  d->SetWorkItem([&callback_d]() { callback_d.Run(); });
  EXPECT_CALL(callback_a, Run()).Times(1);
  EXPECT_CALL(callback_b, Run()).Times(1);
  EXPECT_CALL(callback_c, Run()).Times(1);
  EXPECT_CALL(callback_d, Run()).Times(1);
  
  auto shared_a = thread_pool()->Schedule(std::move(a)).lock();
  EXPECT_NE(shared_a, nullptr);
  b->AddDependency(shared_a);
  auto shared_b = thread_pool()->Schedule(std::move(b)).lock();
  EXPECT_NE(shared_b, nullptr);
  auto shared_c = thread_pool()->Schedule(std::move(c)).lock();
  EXPECT_NE(shared_c, nullptr);
  d->AddDependency(shared_b);
  d->AddDependency(shared_c);
  auto shared_d = thread_pool()->Schedule(std::move(d)).lock();
  EXPECT_NE(shared_d, nullptr);
  EXPECT_EQ(shared_b->GetState(), Task::DISPATCHED);
  EXPECT_EQ(shared_d->GetState(), Task::DISPATCHED);
  thread_pool()->RunNext();
  EXPECT_EQ(shared_a->GetState(), Task::COMPLETED);
  EXPECT_EQ(shared_b->GetState(), Task::DEPENDENCIES_COMPLETED);
  EXPECT_EQ(shared_c->GetState(), Task::DEPENDENCIES_COMPLETED);
  thread_pool()->RunNext();
  thread_pool()->RunNext();
  EXPECT_EQ(shared_b->GetState(), Task::COMPLETED);
  EXPECT_EQ(shared_c->GetState(), Task::COMPLETED);
  EXPECT_EQ(shared_d->GetState(), Task::DEPENDENCIES_COMPLETED);
  thread_pool()->RunNext();
  EXPECT_EQ(shared_d->GetState(), Task::COMPLETED);
}

(1). 执行一个任务

0.初始化一个线程池 ThreadPool::ThreadPool(int num_threads) thread_pool;
1.设置任务(Task task_one) Task::SetWorkItem(const WorkItem& work_item);
2.将task_one插入到tasks_not_ready_队列中, 并执行task的SetThreadPool()函数 ThreadPool::Schedule(std::unique_ptr task);
3. [后台]没有依赖，执行 ThreadPool::NotifyDependenciesCompleted(Task* task) 将任务 task_one 加入到队列中 task_queue_
4. [后台]其中一个线程执行任务 task_one

(2). 执行具有依赖的任务

/*         c \
 *  a -->  b --> d
 */

a --> b ; b依赖a. d 依赖 c 和 b
0.初始化一个线程池 ThreadPool::ThreadPool(int num_threads) thread_pool;
1.设置任务(Task task_a、task_b、task_c、task_d) Task::SetWorkItem(const WorkItem& work_item);
2.将task_a插入到tasks_not_ready_队列中, 并执行task_a的SetThreadPool()函数 ThreadPool::Schedule(std::unique_ptr task); [后台开始执行task_a]
3.将task_a添加到task_b的依赖中， void Task::AddDependency(std::weak_ptr dependency) | b->AddDependency(shared_a);
4.将task_b插入到tasks_not_ready_队列中, 并执行task_b的SetThreadPool()函数 ThreadPool::Schedule(std::unique_ptr task); [task_a执行完后通知task_b并开始执行]
5.将task_c插入到tasks_not_ready_队列中, 并执行task_c的SetThreadPool()函数 ThreadPool::Schedule(std::unique_ptr task); [后台开始执行task_c]
6.将task_b、task_c添加到task_d的依赖中， void Task::AddDependency(std::weak_ptr dependency) d->AddDependency(shared_b); d->AddDependency(shared_c);
7.将task_d插入到tasks_not_ready_队列中, 并执行task_c的SetThreadPool()函数 ThreadPool::Schedule(std::unique_ptr task); [task_b、task_c执行完后通知task_d并开始执行]

1.2.thread_pool_test

请添加图片描述

对于图中的任务流程，并行运算步骤如下：

TEST(ThreadPoolTest, RunWithMultipleDependencies) {
  ThreadPool pool(2);
  Receiver receiver;
  auto task_1 = absl::make_unique<Task>();
  task_1->SetWorkItem([&receiver]() { receiver.Receive(1); });
  auto task_2a = absl::make_unique<Task>();
  task_2a->SetWorkItem([&receiver]() { receiver.Receive(2); });
  auto task_2b = absl::make_unique<Task>();
  task_2b->SetWorkItem([&receiver]() { receiver.Receive(2); });
  auto task_3 = absl::make_unique<Task>();
  task_3->SetWorkItem([&receiver]() { receiver.Receive(3); });
  /*          -> task_2a \
   *  task_1 /-> task_2b --> task_3
   */
  auto weak_task_1 = pool.Schedule(std::move(task_1));
  task_2a->AddDependency(weak_task_1);
  auto weak_task_2a = pool.Schedule(std::move(task_2a));
  task_3->AddDependency(weak_task_1);  // 和task_test的区别，间接依赖也需要加上
  task_3->AddDependency(weak_task_2a);
  task_2b->AddDependency(weak_task_1);
  auto weak_task_2b = pool.Schedule(std::move(task_2b));
  task_3->AddDependency(weak_task_2b);
  pool.Schedule(std::move(task_3));
  receiver.WaitForNumberSequence({1, 2, 2, 3});
}

2.cartographer线程池使用

整个系统中定义了唯一的一个线程池对象：

请添加图片描述

从前面的线程池使用逻辑可以得出：

通过Schedule函数来查看有哪些任务；
通过AddDependency函数查看依赖关系。

分别以这两个函数名为关键字进行搜索[只看2d]：

请添加图片描述

整理一下: 在pose_graph_2d.cc中函数AddWorkItem调用了Schedule:

// 将任务放入到任务队列中等待被执行
void PoseGraph2D::AddWorkItem(
    const std::function<WorkItem::Result()>& work_item) {
  absl::MutexLock locker(&work_queue_mutex_);

  if (work_queue_ == nullptr) {
    // work_queue_的初始化
    work_queue_ = absl::make_unique<WorkQueue>();
    // 将 执行一次DrainWorkQueue()的任务 放入线程池中等待计算
    auto task = absl::make_unique<common::Task>();
    task->SetWorkItem([this]() { DrainWorkQueue(); });
    thread_pool_->Schedule(std::move(task));
  }

  const auto now = std::chrono::steady_clock::now();
  // 将传入的任务放入work_queue_队列中
  work_queue_->push_back({now, work_item});

  kWorkQueueSizeMetric->Set(work_queue_->size());
  kWorkQueueDelayMetric->Set(
      std::chrono::duration_cast<std::chrono::duration<double>>(
          now - work_queue_->front().time)
          .count());
}

在pose_graph_2d又定义了个概念： work_queue_。函数 AddWorkItem 将 work_item 加入到 work_queue_ 队列中，真正执行 work_item 是在 DrainWorkQueue 中，DrainWorkQueue 也作为任务加入到线程池中：

task->SetWorkItem([this]() { DrainWorkQueue(); });

void PoseGraph2D::DrainWorkQueue()
{
  bool process_work_queue = true;
  size_t work_queue_size;
  while (process_work_queue)
  {
    std::function<WorkItem::Result()> work_item;
    {
      absl::MutexLock locker(&work_queue_mutex_);
      // work_queue_ 队列中取出压栈的任务一个一个执行，直到为空
      // work_queue_ 为空, 说明thread_pool中DrainWorkQueue执行完,
      // 等待下一次addNode时候,再次执行thread_pool_->Schedule(task),再次执行.
      if (work_queue_->empty())
      {
        work_queue_.reset();  // 智能指针 reset
        return;
      }
      // 获取队列中最前的任务，这就是上面说的 work_item
      work_item = work_queue_->front().task;
      work_queue_->pop_front();
      work_queue_size = work_queue_->size();
      kWorkQueueSizeMetric->Set(work_queue_size);
    }
    //这里的 work_item()就是我们需要的 AddImuData里的lambda表达式
    //回头看lambda，可以发现返回值是 kDoNotRunOptimization
    // 执行工作任务后，返回的状态，赋值false或true给 process_work_queue
    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
  // 运行到这里，说明队列里运行的lambda表达式的返回值是 kRunOptimization
  // 实际上pop了好几个任务
  //  如果work_item()都不需要全局优化, 则直到work_queue_为空, 都不会执行优化
  LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
  // We have to optimize again
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result)
      {
        HandleWorkQueue(result);
      }  );
}

PoseGraph2D有16个成员函数调用了AddWorkItem，但是只有3个函数会返回 kRunOptimization：
FinishTrajectory, RunFinalOptimization和 ComputeConstraintsForNode,也就是执行完这三个函数后会跳出 while (process_work_queue) 循环，执行 WhenDone 【优化】。

依赖关系总结如下：

请添加图片描述

16 个调用 AddWorkItem 的函数：

请添加图片描述

图自

16 个调用 AddWorkItem 函数中最重要的是后端入口函数PoseGraph2D::AddNode，在它的work_item是ComputeConstraintsForNode，在ComputeConstraintsForNode的最后部分：

if (options_.optimize_every_n_nodes() > 0 &&
  num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes() )
{
	return  WorkItem::Result::kRunOptimization;
}
return  WorkItem::Result::kDoNotRunOptimization;

整个SLAM中实时进入任务队列的主要为计算约束，当加入节点数超过参数时，才进行一次优化。

参考文章

3.执行优化时的梳理

step1: pose_graph_2d 里面调用WhenDone()函数，参数为一个lambda函数

// 在调用线程上执行工作队列中的待处理任务, 直到队列为空或需要优化时退出循环
void PoseGraph::DrainWorkQueue() {
  bool process_work_queue = true;
  // 循环一直执行, 直到队列为空或需要优化时退出循环
  while (process_work_queue) {
	  ...
      // 退出条件1 如果任务队列空了, 就将work_queue_的指针删除
      if (work_queue_->empty()) {
        work_queue_.reset();
        return;
 	  ...
    }
    // 执行任务
    // 退出条件2 执行任务后的结果是需要优化, process_work_queue为false退出循环
    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
  // 退出循环后, 首先等待计算约束中的任务执行完, 再执行HandleWorkQueue,进行优化
  constraint_builder_.WhenDone(
      [this](const mapping::ConstraintBuilder::Result& result) {
        HandleWorkQueue(result);
      });
}

step2：constraint_builder_2d 里面将RunWhenDoneCallback函数（内部调用lambda函数）设置为任务放入线程池中运行

// 约束计算完成之后执行一下回调函数
void ConstraintBuilder::WhenDone(
    const std::function<void(const ConstraintBuilder::Result&)>& callback) {
  absl::MutexLock locker(&mutex_);
  CHECK(when_done_ == nullptr);

  // TODO(gaschler): Consider using just std::function, it can also be empty.
  // 将回调函数赋值给when_done_
  when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback);
  CHECK(when_done_task_ != nullptr);

  // 生成 执行when_done_的任务
  when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); });
  // 将任务放入线程池中等待执行
  thread_pool_->Schedule(std::move(when_done_task_));
  LOG(WARNING) << "Schedule when_done_";

  // when_done_task_的重新初始化
  when_done_task_ = absl::make_unique<common::Task>();
}

step3：constraint_builder_2d 里面RunWhenDoneCallback函数调用lambda函数

// 将临时保存的所有约束数据传入回调函数, 并执行回调函数
void ConstraintBuilder::RunWhenDoneCallback() {
  Result result;
  std::unique_ptr<std::function<void(const Result&)>> callback;
  {
    absl::MutexLock locker(&mutex_);
    CHECK(when_done_ != nullptr);

    // 将计算完的约束进行保存
    for (const std::unique_ptr<Constraint>& constraint : constraints_) {
      if (constraint == nullptr) continue;
      result.push_back(*constraint);
    }

    if (options_.log_matches) {
      LOG(INFO) << constraints_.size() << " computations resulted in "
                << result.size() << " additional constraints.";
      LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
    }

    // 这些约束已经保存过了, 就可以删掉了
    constraints_.clear();

    callback = std::move(when_done_);
    when_done_.reset();
    kQueueLengthMetric->Set(constraints_.size());
  }
  // 执行回调函数 HandleWorkQueue
  (*callback)(result);
}

lambda函数：

  [this](const mapping::ConstraintBuilder::Result& result) {
        HandleWorkQueue(result);
      }

ConstraintBuilder::WhenDone(): lambda函数–>callback–>when_done_
ConstraintBuilder::RunWhenDoneCallback(): when_done_–>callback–>(*callback)(result);[调用执行]

古路

关注

1
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
cartographer_thread_pool

整个SLAM中实时进入任务队列的主要为计算约束，当加入节点数超过参数时，才进行一次优化。线程池看源代码更好理解，这里就不贴代码了。看看测试用例，了解使用过程。[task_b、task_c执行完后通知task_d并开始执行][task_a执行完后通知task_b并开始执行]循环，执行 WhenDone 【优化】。,也就是执行完这三个函数后会跳出。[后台开始执行task_a][后台开始执行task_c]
复制链接

扫一扫