C++?Cartographer源碼中關于Sensor的數據走向分析

本篇內容主要講解“C++ Cartographer源碼中關于Sensor的數據走向分析”，感興趣的朋友不妨來看看。本文介紹的方法操作簡單快捷，實用性強。下面就讓小編來帶大家學習“C++ Cartographer源碼中關于Sensor的數據走向分析”吧!

前言

詳細了解了MapBuilder類, 發現其構造函數, 以及AddTrajectory中有使用到SensorBridge這個類還有sensor_collator_這個變量, 并且似乎是用這個類進行傳感器數據的傳遞的. 當然啦, 如果想建立一個完整的軌跡(Trajectory)和SLAM功能, 我們肯定需要有傳感器的數據灌入的.

在MapBuilder的入口類-MapBuilderBridge中, 可以看到一個變量sensor_bridges_

std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

sensor_bridges_存儲了一系列SensorBridge類的實例, 并且在MapBuilderBridge::AddTrajectory

的第二步使用, 作用是為當前軌跡添加一個SensorBridge.

所以我們重點看看SensorBridge這個類. 我們以最重要的sensor類-LaserScan為例子

Node類的HandleLaserScanMessage函數

我們都知道, ros中軟實時的程序數據的入口都是從subscriber的回調函數開始. 之前已經講過了Node的LaunchSubscriber函數, 用來專門啟動所有傳感器的訂閱. 咱們就看看Node類的HandleLaserScanMessage:

// 調用SensorBridge的傳感器處理函數進行數據處理
void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 根據配置,是否將傳感器數據跳過
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

我們可以看到, 他最終是調用了MapBuilderBridge類的sensor_bridge的成員函數-HandleLaserScanMessage來處理傳入的傳感器的數據.

SensorBridge類的HandleLaserScanMessage函數

咱們再去SensorBridge中看看, 以下是主要代碼部分

// 處理LaserScan數據, 先轉成點云,再傳入trajectory_builder_
void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
    if (sensor_to_tracking != nullptr) {
        trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
    time,
    sensor_to_tracking->translation().cast<float>(),
    carto::sensor::TransformTimedPointCloud(
    ranges, sensor_to_tracking->cast<float>())} ); // 強度始終為空
    }
}

前幾節也講過這部分SensorBridge::HandleLaserScanMessage調用了SensorBridge::HandleRangefinder, 把carto::sensor::TimedPointCloudData這個數據類型和sensor_id通過trajectory_builder_的AddSensorData把點云類型傳遞給CollatedTrajectoryBuilder的AddSensorData. 為啥是CollatedTrajectoryBuilder的AddSensorData呢?這塊我也想了很久. 咱們先去sensor_bridge.h中看

  ::cartographer::mapping::TrajectoryBuilderInterface* const
      trajectory_builder_;

發現是TrajectoryBuilderInterface, 這明顯是個父類啊,沒啥意義. 咱們回到SensorBridge的構造函數

/**
 * @brief 構造函數, 并且初始化TfBridge
 * 
 * @param[in] num_subdivisions_per_laser_scan 一幀數據分成幾次發送
 * @param[in] tracking_frame 數據都轉換到tracking_frame
 * @param[in] lookup_transform_timeout_sec 查找tf的超時時間
 * @param[in] tf_buffer tf_buffer
 * @param[in] trajectory_builder 軌跡構建器
 */
SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {}

發現這個trajectory_builder_是SensorBridge構造函數的最后一個參數, 那么這個SensorBridge是在哪構造的呢? 是在map_builder_bridge.cc中, MapBuilderBridge::AddTrajectory的第二步sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>. 我們看到最后一個參數是

map_builder_->GetTrajectoryBuilder(trajectory_id)

這個map_builder_是MapBuilderBridge構造函數map_builder_(std::move(map_builder)), 而這個map_builder也是父類定義,沒啥參考價值

std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder

再向前回溯, 看MapBuilderBridge是咋構造的, 發現是Node的構造函數就構造了map_builder_bridge_

map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)

又要往前回溯, 在node_main.cc中

 auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
 Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);

發現map_builder是cartographer::mapping::CreateMapBuilder給的. 咱們再進CreateMapBuilder, 發現其只是一個工廠函數

std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

而這個工廠函數實例化了MapBuilder這個類, 所以map_builder_->GetTrajectoryBuilder(trajectory_id)調用的是MapBuilder的GetTrajectoryBuilder. (有一種峰回路轉的感覺), 返回trajectory_builders_的軌跡id為trajectory_id的指針,即:

  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

而trajectory_builders_在map_builder.cc中被壓入absl::make_unique<CollatedTrajectoryBuilder>, 如下

trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 將3D前端與3D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder3D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph4D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

所以最終SensorBridge的trajectory_builder_實際上是CollatedTrajectoryBuilder的地址, 所以SensorBridge的trajectory_builder_的AddSensorData實際上是把數據添加到了CollatedTrajectoryBuilder里面, 而不是GlobalTrajectoryBuilder或者LocalTrajectoryBuilder(這三個TrajectoryBuilder都繼承于TrajectoryBuilderInterface)

這塊地方難就難在子類可以用父類代替, 搞不清到底是調用的哪個子類的成員函數.

CollatedTrajectoryBuilder類的AddSensorData函數

既然上面調用的是CollatedTrajectoryBuilder, 那咱們看看CollatedTrajectoryBuilder這個類的AddSensorData

void AddSensorData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
    sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

再看看sensor::MakeDispatchable, 在dispatchable.h文件中

// 根據傳入的data的數據類型,自動推斷DataType, 實現一個函數處理不同類型的傳感器數據
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
    const std::string &sensor_id, const DataType &data) {
  return absl::make_unique<Dispatchable<DataType>>(sensor_id, data);
}

這個函數通過模板, 實現了一個函數處理多個類型, 也就是說可以用一個函數去分發上到激光雷達,下到IMU的數據, 值得學習.

CollatedTrajectoryBuilder::AddData又調用sensor_collator_->AddSensorData, 用std::move(data), 把data移動給AddSensorData, 給某個Trajectory加入傳感器數據. 而這個sensor_collator_定義如下:

sensor::CollatorInterface* const sensor_collator_;

又是用父類代替子類, 在CollatedTrajectoryBuilder的構造函數中實現實例化, 這個sensor_collator_實際上是sensor::Collator, 原因是在map_builder.cc中的MapBuilder構造函數中有如下一段程序

  // 在 cartographer/configuration_files/map_builder.lua 中設置
  // param: MAP_BUILDER.collate_by_trajectory 默認為false
  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    // sensor_collator_初始化, 實際使用這個
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }

一般collate_by_trajectory設置為false, 所以是absl::make_unique<sensor::Collator>, 即sensor的Collator

  // sensor::Collator的初始化
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        HandleCollatedSensorData(sensor_id, std::move(data)); //傳遞給GlobalTrajectoryBuilder類相應的函數
      });

Collator類的AddSensorData函數

咱們進到collator這個類中看看, 發現這個類繼承于CollatorInterface, 再看看collator這個類的AddSensorData

// 向數據隊列中添加 傳感器數據 
void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data) {
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  queue_.Add(std::move(queue_key), std::move(data));
}

作用是 向隊列中添加傳感器數據, 啥是隊列?以后將在線程池部分詳細說說. 現在簡單看看

queue_是Cartographer的任務隊列, 用于線程池多任務序列的儲存與處理.

// Queue keys are a pair of trajectory ID and sensor identifier.
  OrderedMultiQueue queue_;

也就是說Collator::AddSensorData負責把data放在任務隊列中等待處理并賦一個key, 并不負責處理數據, 所以咱們再往前看看, 看一下OrderedMultiQueue這個類的關于添加數據的成員函數-Add

OrderedMultiQueue類的Add函數

OrderedMultiQueue這個類定義在ordered_multi_queue.cc中, 添加數據是在Add成員函數實現的:

// 向數據隊列中添加數據
void OrderedMultiQueue::Add(const QueueKey& queue_key,
                            std::unique_ptr<Data> data) {
  auto it = queues_.find(queue_key);
  // 如果queue_key不在queues_中, 就忽略data
  if (it == queues_.end()) {
    LOG_EVERY_N(WARNING, 1000)
        << "Ignored data for queue: '" << queue_key << "'";
    return;
  }
  // 向數據隊列中添加數據
  it->second.queue.Push(std::move(data));
  // 傳感器數據的分發處理
  Dispatch();
}

可以發現Add就是生產者, 用于生成并傳遞可用數據.

Dispatch()這個成員函數負責數據分發, 將處于數據隊列中的數據根據時間依次傳入回調函數. 這個后面再看, 咱們先看看it->second.queue.Push(std::move(data));這個部分.

it這個變量就是queues_最后一個數據,可以理解為最新的一個數據, 而OrderedMultiQueue的queue_和上一小節提到的Collator是不同的, 在OrderedMultiQueue中的queue_是定義為一個std::map

std::map<QueueKey, Queue> queues_; // 多個數據隊列

所以it->second就是Queue, 而Queue是個定義在OrderedMultiQueue的結構體

  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;   // 存儲數據的隊列
    Callback callback;                                    // 本數據隊列對應的回調函數
    bool finished = false;                                // 這個queue是否finished
  };

Push 也就是把data壓入Queue這個結構體中,然后生成map形成個對列. 而這個Push不是push_back, 這個Push是Cartographer自己定義的一種壓棧方法. 定義在blocking_queue.h中,如下...

BlockingQueue類的Push函數

  // Pushes a value onto the queue. Blocks if the queue is full.
  // 將值壓入隊列. 如果隊列已滿, 則阻塞
  void Push(T t) {
    // 首先定義判斷函數
    const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
      return QueueNotFullCondition();
    };
    // absl::Mutex的更多信息可看: https://www.jianshu.com/p/d2834abd6796
    // absl官網: https://abseil.io/about/
    // 如果數據滿了, 就進行等待
    absl::MutexLock lock(&mutex_);
    mutex_.Await(absl::Condition(&predicate));
    // 將數據加入隊列, 移動而非拷貝
    deque_.push_back(std::move(t));
  }

發現Push作用相當于阻塞者, 使用了mutex_.Await和鎖用來阻塞數據傳入. 看看QueueNotFullCondition這個函數就一目了然了. 當隊列為無限大或者小于queue_size_的時候返回true.

  // Returns true iff the queue is not full.
  // 如果隊列未滿, 則返回true
  bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
    return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
  }

除了阻塞作用, 最大的所用就是把數據壓入deque_, 咱們再看看這個deque_

template <typename T>
...
std::deque<T> deque_ GUARDED_BY(mutex_);

發現它是std::deque這個基礎類型,類型決定于模板T, GUARDED_BY(mutex_)表示這個數據在使用的時候必須要上鎖, 否則就會報錯.

所以Push的作用就是有阻塞作用的push_back, 負責把data壓入OrderedMultiQueue的queue_, 并且在隊列滿的時候阻塞.

OrderedMultiQueue類的Dispatch函數

咱們再回到OrderedMultiQueue類的Add函數的Dispatch中, 看看Dispatch函數有關數據分發的部分

void OrderedMultiQueue::Dispatch() {
    while (true) {
        const Data* next_data = nullptr;
        Queue* next_queue = nullptr;
        QueueKey next_queue_key;
        // 遍歷所有的數據隊列, 找到所有數據隊列的第一個數據中時間最老的一個數據
        for (auto it = queues_.begin(); it != queues_.end();) {
            const auto* data = it->second.queue.Peek<Data>();
        } // end for
        // 正常情況, 數據時間都超過common_start_time
        if (next_data->GetTime() >= common_start_time) {
            last_dispatched_time_ = next_data->GetTime();
            // 將數據傳入 callback() 函數進行處理,并將這個數據從數據隊列中刪除
            next_queue->callback(next_queue->queue.Pop());
        }
    }
}

Peek是取出隊列最前面的一個數據. callback定義在頭文件中, 這個是std::function封裝的一個函數

using Callback = std::function<void(std::unique_ptr<Data>)>;

而Callback這個函數到底是啥呢? 這個要看OrderedMultiQueue::AddQueue這個成員函數

void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0);
  queues_[queue_key].callback = std::move(callback);
}

我們看到這個函數把參數傳入的callback傳入queues_的callback. 那么是誰調用的OrderedMultiQueue的AddQueue這個函數呢? 是Collator的AddTrajectory調用的!

我們在回溯到Collator這個類看看Collator的AddTrajectory成員函數

/**
 * @brief 添加軌跡以生成排序的傳感器輸出, 每個topic設置一個回調函數
 * 
 * @param[in] trajectory_id 新生成的軌跡的id
 * @param[in] expected_sensor_ids 需要排序的topic名字的集合
 * @param[in] callback 2個參數的回調函數, 實際是CollatedTrajectoryBuilder::HandleCollatedSensorData()函數
 */
void Collator::AddTrajectory(
    const int trajectory_id,
    const absl::flat_hash_set<std::string>& expected_sensor_ids,
    const Callback& callback) {
  for (const auto& sensor_id : expected_sensor_ids) {
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    queue_.AddQueue(queue_key,
                    // void(std::unique_ptr<Data> data) 帶了個默認參數sensor_id
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

我們看到它調用了queue_的AddQueue, 而queue_就是OrderedMultiQueue的實例化 ,所以這里的AddQueue是OrderedMultiQueue的AddQueue. Callback又是個lambda函數

[callback, sensor_id](std::unique_ptr<Data> data) {
     callback(sensor_id, std::move(data));
}

這個lambda函數調用的是傳入的callback函數, 而這個Collator::AddTrajectory是誰調用的呢?實際上是CollatedTrajectoryBuilder. 在CollatedTrajectoryBuilder的構造函數中就實現了Collator這個類的初始化, 并且調用了AddTrajectory這個函數

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    const proto::TrajectoryBuilderOptions& trajectory_options,
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder) ...
{
    ...
    // sensor::Collator的初始化
    sensor_collator_->AddTrajectory(
    trajectory_id, expected_sensor_id_strings,
    [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
      HandleCollatedSensorData(sensor_id, std::move(data)); //傳遞給GlobalTrajectoryBuilder類相應的函數
    });
}

所以傳入的參數是是HandleCollatedSensorData這個函數.

CollatedTrajectoryBuilder類的HandleCollatedSensorData函數

這個函數才是真正的消費者. 看一下HandleCollatedSensorData傳入sensor data的部分:

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
    // 將排序好的數據送入 GlobalTrajectoryBuilder中的AddSensorData()函數中進行使用
    data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

這個函數的作用是處理按照時間順序分發的傳感器數據, 在進去到Data的AddToTrajectoryBuilder里看看

Data這個類又是個基類, 里面有個純虛函數

  virtual void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;

這個基類只有一個子類: Dispatchable. 進到這個子類中去看看AddToTrajectoryBuilder.

  // 調用傳入的trajectory_builder的AddSensorData()
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

所以這里的trajectory_builder指的就是CollatedTrajectoryBuilder::HandleCollatedSensorData中調用的wrapped_trajectory_builder_. 而這個wrapped_trajectory_builder_是啥呢?這又要回溯到CollatedTrajectoryBuilder的初始構造中去, 在map_builder.cc中實現

trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( 
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 將2D前端與2D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局軌跡構建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個父類
            std::move(local_trajectory_builder), //前端構建器
            trajectory_id, //
            static_cast<PoseGraph3D*>(pose_graph_.get()), //后端位姿圖
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

我們看到wrapped_trajectory_builder_實際上是CreateGlobalTrajectoryBuilder2D, 這個在上回也說到就是GlobalTrajectoryBuilder這個類CreateGlobalTrajectoryBuilder2D, 返回Cartographer的前端和后端.

到這里, 整個Cartographer的傳感器數據傳遞過程也就明了了.

從GlobalTrajectoryBuilder2D開始, 數據才真正走到SLAM的前端與后端部分.

到此，相信大家對“C++ Cartographer源碼中關于Sensor的數據走向分析”有了更深的了解，不妨來實際操作一番吧！這里是億速云網站，更多相關內容可以進入相關頻道進行查詢，關注我們，繼續學習！