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的构造函数

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成员函数

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的数据走向分析”有了更深的了解，不妨来实际操作一番吧！这里是编程网网站，更多相关内容可以进入相关频道进行查询，关注我们，继续学习！