C++ Cartographer源码中关于Sensor的数据走向深扒

目录

• 前言
• Node类的HandleLaserScanMessage函数
• SensorBridge类的HandleLaserScanMessage函数
• CollatedTrajectoryBuilder类的AddSensorData函数
• Collator类的AddSensorData函数
• OrderedMultiQueue类的Add函数
• BlockingQueue类的Push函数
• OrderedMultiQueue类的Dispatch函数
• CollatedTrajectoryBuilder类的HandleCollatedSensorData函数
• 总结

前言

整个Cartographer源码阅读是很枯燥的, 但绝对是可以学到东西的! 坚持每天记录, 加油!

在上一节我们详细了解了MapBuilder类, 发现其构造函数, 以及AddTrajectory中有使用到SensorBridge这个类还有sensor_collator_这个变量, 并且似乎是用这个类进行传感器数据的传递的. 当然啦, 如果想建立一个完整的轨迹(Trajectory)和SLAM功能, 我们肯定需要有传感器的数据灌入的.

在MapBuilder的入口类-MapBuilderBridge中, 可以看到一个变量sensor_bridges_

std::unordered_map> 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(),
    carto::sensor::TransformTimedPointCloud(
    ranges, sensor_to_tracking->cast())} ); // 强度始终为空
    }
}

前几节也讲过这部分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. 我们看到最后一个参数是

map_builder_->GetTrajectoryBuilder(trajectory_id)

这个map_builder_是MapBuilderBridge构造函数map_builder_(std::move(map_builder)), 而这个map_builder也是父类定义,没啥参考价值

std::unique_ptr 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 CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique(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, 如下

trajectory_builders_.push_back(absl::make_unique(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder3D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast(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 data) {
    sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

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

// 根据传入的data的数据类型,自动推断DataType, 实现一个函数处理不同类型的传感器数据
template 
std::unique_ptr> MakeDispatchable(
    const std::string &sensor_id, const DataType &data) {
  return absl::make_unique>(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();
  } else {
    // sensor_collator_初始化, 实际使用这个
    sensor_collator_ = absl::make_unique();
  }

一般collate_by_trajectory设置为false, 所以是absl::make_unique, 即sensor的Collator

  // sensor::Collator的初始化
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr 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) {
  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) {
  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 queues_; // 多个数据队列

所以it->second就是Queue, 而Queue是个定义在OrderedMultiQueue的结构体

  struct Queue {
    common::BlockingQueue> 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 
...
std::deque 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();
        } // 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)>;

而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& 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) 带了个默认参数sensor_id
                    [callback, sensor_id](std::unique_ptr 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) {
     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& expected_sensor_ids,
    std::unique_ptr wrapped_trajectory_builder) ...
{
    ...
    // sensor::Collator的初始化
    sensor_collator_->AddTrajectory(
    trajectory_id, expected_sensor_id_strings,
    [this](const std::string& sensor_id, std::unique_ptr 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 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( 
        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(pose_graph_.get()), //后端位姿图
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

我们看到wrapped_trajectory_builder_实际上是CreateGlobalTrajectoryBuilder2D, 这个在上回也说到就是GlobalTrajectoryBuilder这个类CreateGlobalTrajectoryBuilder2D, 返回Cartographer的前端和后端.

到这里, 整个Cartographer的传感器数据传递过程也就明了了.

从GlobalTrajectoryBuilder2D开始, 数据才真正走到SLAM的前端与后端部分.

总结

Cartographer中传感器数据的传入从Node类的HandleXXXMessage成员函数开始, 传递给SensorBridge类, 然后调用CollatedTrajectoryBuilder把数据给到Collator类, 由Collator进行消费这模式的处理, 然后再返回给CollatedTrajectoryBuilder完成Cartographer的整个前后端.

中间的处理用到了很多父子类的互相调用, 一层套一层, 十分复杂, 要认真看才能懂里面的数据流.

到此这篇关于C++ Cartographer源码中关于Sensor的数据走向深扒的文章就介绍到这了,更多相关C++ Sensor数据走向内容请搜索脚本之家以前的文章或继续浏览下面的相关文章希望大家以后多多支持脚本之家！