cartographer中传感器数据的处理

cartographer中传感器数据的分发处理

传感器数据走向分析

Node类的HandleLaserScanMessage函数

void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

这个函数是订阅LaserScan的topic绑定的回调函数,这个函数执行的是map_builder_bridge里的sensor_bridge里的HandleLaserScanMessage这个函数。

sensor_bridge里的HandleLaserScanMessage函数

// 处理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);
}

这个函数是将激光雷达的点云数据加上时间和强度信息,并将处理后的点云数据以及sensor_id,frame_id,time一起传入HandleLaserScan这个函数。

HandleLaserScan这个函数最后是调用了HandleRangefinder进行处理。

HandleRangefinder函数

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>(),
                       // 将点云从雷达坐标系下转到tracking_frame坐标系系下
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空
  }
}

这个函数也没有对数据进行实际的处理,而是调用trajectory_builder_->AddSensorData这个函数,对点云的坐标进行坐标变换。

SensorBridge的*trajectory_builder_实际也是指向CollatedTrajectoryBuilder的指针

这个类不区分2d与3d,整理传感器数据,将传感器数据按照时间顺序进行排列,然后发送到GlobalTrajectoryBuilder。

CollatedTrajectoryBuilder在MapBuilder::AddTrajectoryBuilder中初始化

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
    
...

trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
  }

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));
  }

这个函数调用AddData函数添加数据

CollatedTrajectoryBuilder类的AddData函数

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

将数据传入sensor_collator_的AddSensorData进行排序

sensor_collator_的初始化

在MapBuilder的构造函数中对sensor_collator_进行初始化

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) {
  ...
  // 在 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>();
  }
}

然后再MapBuilder类的AddTrajectoryBuilder函数中将sensor_collator_的指针传给了CollatedTrajectoryBuilder这个类。

CollatedTrajectoryBuilder的sensor_collator_是指向Collator的指针,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));
}

这个函数完成的是向数据队列中添加传感器数据,将原始的data以及封装好的queue_key传入到queue.Add

上述的Add函数在 OrderedMultiQueue类中

OrderedMultiQueue类的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();
}

传感器数据进入Dispatch函数进行分发处理

OrderedMultiQueue类的Dispatch函数

文档最后对Dispatch函数做详解,这里只说明数据的传输流程。

void OrderedMultiQueue::Dispatch() {
  while (true) {
    const Data* next_data = nullptr;
    Queue* next_queue = nullptr;
    QueueKey next_queue_key;
    ...
     if (next_data->GetTime() >= common_start_time) {
      // Happy case, we are beyond the 'common_start_time' already.
      // 更新分发数据的时间
      last_dispatched_time_ = next_data->GetTime();
      // 将数据传入 callback() 函数进行处理,并将这个数据从数据队列中删除
      next_queue->callback(next_queue->queue.Pop());

callback函数是在Collator::AddTrajectory()中使用labmda表达式传参

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);
  }
}

callback是CollatedTrajectoryBuilder::HandleCollatedSensorData这个函数

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

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

trajectory_builder实际上就是GlobalTrajectoryBuilder的指针

从GlobalTrajectoryBuilder开始,数据就真正走到了slam的前端与后端部分。

注:数据首先都是先传入缓冲区,然后从缓冲区里面读取,缓冲区处理代码位于:cartographer/cartographer/common/internal/blocking_queue.h

CollatedTrajectoryBuilder类

代码路径:cartographer/mapping/internal/collated_trajectory_builder.cc

这个类不区分2d与3d,整理传感器数据,将传感器数据按照时间顺序进行排列,然后发送到GlobalTrajectoryBuilder。

collator类

collator::AddTrajectory

在CollatedTrajectoryBuilder的构造函数中调用

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));
      });

进入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);
  }
}

其中QueueKey在ordered_multi_queue.h中定义

struct QueueKey {
  int trajectory_id;      // 轨迹id
  std::string sensor_id;  // topic名字

  // 重载小于运算符, map根据这个规则对QueueKey进行排序
  // 以tuple规则比较2者, tuple定义了<运算符, 逐个元素进行比较
  bool operator<(const QueueKey& other) const {
    return std::forward_as_tuple(trajectory_id, sensor_id) <
           std::forward_as_tuple(other.trajectory_id, other.sensor_id);
  }
};

重载小于运算符, map根据这个规则对QueueKey进行排序,以tuple规则比较2者, tuple定义了<运算符, 逐个元素进行比较

Collator::FinishTrajectory

void Collator::FinishTrajectory(const int trajectory_id) {
  for (const auto& queue_key : queue_keys_[trajectory_id]) {
    queue_.MarkQueueAsFinished(queue_key);
  }
}

将 trajectory_id 标记为完成

获取到对应trajectory_id的queue_key传入到queue.MarkQueueAsFinished函数中,进行finish的标记操作。

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));
}

Collator::Flush()

// 将所有数据队列标记为已完成,分派所有剩下的传感器数据
// 只能调用一次, 在 Flush 之后不能再调用 AddSensorData()
void Collator::Flush() { queue_.Flush(); }

Collator::GetBlockingTrajectoryId()

// 返回在 CollatorInterface 解锁之前需要更多数据的轨迹的 ID
// 对于不等待特定轨迹的实现, 返回 'nullopt'
absl::optional<int> Collator::GetBlockingTrajectoryId() const {
  return absl::optional<int>(queue_.GetBlocker().trajectory_id);
}

数据分发

OrderedMultiQueue类的Dispatch函数

void OrderedMultiQueue::Dispatch() {
  while (true) {
    /*
      queues_: 
        (0, scan): {      4,     }
        (0, imu):  {1,  3,   5,  }
        (0, odom): {  2,       6,}
    */
    const Data* next_data = nullptr;
    Queue* next_queue = nullptr;
    QueueKey next_queue_key;

    // Step: 1 遍历所有的数据队列, 找到所有数据队列的第一个数据中时间最老的一个数据
    for (auto it = queues_.begin(); it != queues_.end();) {

      // c++11: auto*(指针类型说明符), auto&(引用类型说明符), auto &&(右值引用)

      // 获取当前队列中时间最老的一个的一个数据
      const auto* data = it->second.queue.Peek<Data>();

      if (data == nullptr) {
        // 如果队列已经处于finished状态了, 就删掉这个队列
        if (it->second.finished) {
          queues_.erase(it++);
          continue;
        }
        // 退出条件1: 某个话题的数据队列为空同时又不是完成状态, 就先退出, 发布log并标记为阻塞者
        CannotMakeProgress(it->first);
        return;
      }

      // 第一次进行到这里或者data的时间比next_data的时间小(老数据)
      // 就更新next_data, 并保存当前话题的数据队列以及queue_key
      if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
        next_data = data;
        next_queue = &it->second;
        next_queue_key = it->first;
      }

      // 数据的时间戳不是按顺序的, 就报错
      CHECK_LE(last_dispatched_time_, next_data->GetTime())
          << "Non-sorted data added to queue: '" << it->first << "'";
      
      ++it;
    } // end for

    // 退出条件2: 只有多队列queues_为空, 才可能next_data==nullptr
    if (next_data == nullptr) {
      CHECK(queues_.empty());
      return;
    }

    // If we haven't dispatched any data for this trajectory yet, fast forward
    // all queues of this trajectory until a common start time has been reached.
    // 如果我们还没有为这个轨迹分配任何数据, 快进这个轨迹的所有队列, 直到达到一个共同的开始时间
    
    // Step: 2 获取对应轨迹id的所有数据队列中的最小共同时间戳, 作为轨迹开始的时间
    const common::Time common_start_time =
        GetCommonStartTime(next_queue_key.trajectory_id);

    // Step: 3 将 next_queue 的时间最老的一个数据传入回调函数进行处理 

    // 大多数情况, 数据时间都会超过common_start_time的
    if (next_data->GetTime() >= common_start_time) {
      // Happy case, we are beyond the 'common_start_time' already.
      // 更新分发数据的时间
      last_dispatched_time_ = next_data->GetTime();
      // 将数据传入 callback() 函数进行处理,并将这个数据从数据队列中删除
      next_queue->callback(next_queue->queue.Pop());
    } 
    // 数据时间小于common_start_time,同时数据队列数据的个数小于2,只有1个数据的情况 罕见
    else if (next_queue->queue.Size() < 2) {
      // 退出条件3: 数据队列数据的个数少,又不是完成状态, 不能确定现在到底是啥情况, 就先退出稍后再处理
      if (!next_queue->finished) {
        // We cannot decide whether to drop or dispatch this yet.
        CannotMakeProgress(next_queue_key);
        return;
      } 
      // 处于完成状态了, 将数据传入 callback() 函数进行最后几个数据的处理
      // 更新分发数据的时间,将数据传入 callback() 进行处理,并将这个数据从数据队列中删除
      last_dispatched_time_ = next_data->GetTime();
      next_queue->callback(next_queue->queue.Pop());
    } 
    // 数据时间小于common_start_time,同时数据队列数据的个数大于等于2个
    else {
      // We take a peek at the time after next data. If it also is not beyond
      // 'common_start_time' we drop 'next_data', otherwise we just found the
      // first packet to dispatch from this queue.

      // 只处理数据在common_start_time的前一个数据, 其他更早的数据会被丢弃掉
      std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
      if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
        // 更新分发数据的时间,将数据传入 callback() 进行处理
        last_dispatched_time_ = next_data->GetTime();
        next_queue->callback(std::move(next_data_owner));
      }
    }
  }
}

这个函数将处于数据队列中的数据,按照时间顺序依次传入回调函数(数据分发)

执行步骤:

step1. 遍历所有的数据队列, 找到所有数据队列的第一个数据中时间最老的一个数据

step2. 获取对应轨迹id的所有数据队列中的最小共同时间戳, 作为轨迹开始的时间

step3. 将 next_queue 的时间最老的一个数据传入回调函数进行处理

有三种退出情况:

  1. 某个话题的数据队列为空同时又不是完成状态, 就退出

    有两种可能,一种是某一个话题的消息始终没有收到,另一种是对这个话题消息使用过快,是的这个话题中没有消息了

  2. 只有多队列queues_为空, 就退出,

queue为空的情况,是在还未执行AddQueue函数的时候,对其进行构造,而执行dispatch是,这个函数早就执行过了,所以,queues_正常不为空。

  1. 数据队列中数据的个数只有1个,又不是完成状态,不能确定状态, 就先退出

    /*
    queues_:
    (0, scan): { 4, }
    (0, imu): {1, 3, 5, }
    (0, odom): { 2, 6,}
    */
    这个注释是对queue_key 的简单的举例
    前面()里的0代表的是trajectory_id是0,第0条轨迹,后面是各个传感器,{}里面是对应的时间戳,可以看出imu是最快的,数据对应的时间戳是1,3,5;scan是最慢的,时间戳是4。

const common::Time common_start_time =
        GetCommonStartTime(next_queue_key.trajectory_id);

找到数据队列中所有第一帧的最大时间(共同时间),对于某个id轨迹的common_start_time只会计算一次。

进入GetCommonStartTime函数文章来源地址https://www.uudwc.com/A/Eva1Y/

common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) {

  // c++11: map::emplace() 返回的 pair 对象
  // pair 的成员变量 first 是一个指向插入元素或阻止插入的元素的迭代器
  // 成员变量 second 是个布尔值, 表示是否插入成功, 如果这个元素的索引已经存在插入会失败,返回false
  auto emplace_result = common_start_time_per_trajectory_.emplace(
      trajectory_id, common::Time::min());
  common::Time& common_start_time = emplace_result.first->second;

  // 如果插入成功了就找到时间戳最大的对common_start_time进行更新, 失败了就不更新
  // 只会在轨迹开始时插入成功一次
  if (emplace_result.second) {
    // 找到这个轨迹下,所有数据队列中数据的时间戳最大 的时间戳
    // 执行到这里时, 所有的数据队列都有值了, 因为没值的情况在Dispatch()中提前返回了
    for (auto& entry : queues_) {
      if (entry.first.trajectory_id == trajectory_id) {
        common_start_time = std::max(
            common_start_time, entry.second.queue.Peek<Data>()->GetTime());
      }
    }
    LOG(INFO) << "All sensor data for trajectory " << trajectory_id
              << " is available starting at '" << common_start_time << "'.";
  }

  return common_start_time;
}

原文地址:https://blog.csdn.net/weixin_44088154/article/details/132956429

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请联系站长进行投诉反馈,一经查实,立即删除!

h
上一篇 2023年09月18日 02:12
Spring Boot 中的 @Controller 注解:原理、用法与示例
下一篇 2023年09月18日 02:12