cartographer 源码解析（二）

左手的ㄟ右手 2022-08-30 04:09 151阅读 0赞

## 相关链接： ##

#### [cartographer 源码解析(一)][cartographer] ####

#### [cartographer 源码解析(二)][cartographer 1] ####

#### [cartographer 源码解析(三)][cartographer 2] ####

#### [cartographer 源码解析(四)][cartographer 3] ####

#### [cartographer 源码解析(五)][cartographer 4] ####

#### [cartographer 源码解析(六)][cartographer 5] ####

上一次讲了建立数据处理流水线，等待数据处理：[链接][cartographer]  
现在开始从订阅器的回调函数入手，跟着数据跟踪它的流向，我们举一个例子，二维激光数据的例子吧。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]

我们还是先从node.cc入手，翠花，上代码。  
**node.cc**

int Node::AddTrajectory(const TrajectoryOptions& options) {
        
    ...
      AddExtrapolator(trajectory_id, options);
      AddSensorSamplers(trajectory_id, options);
      LaunchSubscribers(options, trajectory_id);
     ...
      return trajectory_id;
    }

我们在**node.cc**中看到有一个 `LaunchSubscribers(options, trajectory_id);`订阅函数，让它把订阅器都给订阅起来。那么在订阅激光的时候。有一个`HandleLaserScanMessage`，  
**node.cc**

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                                 const int trajectory_id) {
        
      for (const std::string& topic :
           ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
        
        subscribers_[trajectory_id].push_back(
            {
        SubscribeWithHandler<sensor_msgs::LaserScan>(
                 &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
                 this),
             topic});
      }
      for (const std::string& topic : ComputeRepeatedTopicNames(
               kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
        
        subscribers_[trajectory_id].push_back(
            {
        SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
                 &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
                 &node_handle_, this),
             topic});
      }
      for (const std::string& topic :
           ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
        
        subscribers_[trajectory_id].push_back(
            {
        SubscribeWithHandler<sensor_msgs::PointCloud2>(
                 &Node::HandlePointCloud2Message, trajectory_id, topic,
                 &node_handle_, this),
             topic});
      }

我们跟进`HandleLaserScanMessage`进去看一下。  
**node.cc**

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

用于处理激光的消息，第一步是各个消息之间的锁，防止它们互相干扰，第二部是一个数据采样器，防止频率高**sensor\_samplers\_**。不满足就**return**。再通过**map\_builder\_bridge\_去处理数据。要牢记，我们node**里面最重要的就是**map\_builder\_bridge\_**，在**map\_builder\_bridge\_里面有一个sensor\_bridge**，数据格式转化器，去处理**ScanMessage**。找到**ScanMessage**看**sensor->bridge**是怎么处理的。  
**sensor\_bridge.cc**

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

用**carto**命名空间里的**sensor**里的**PointCloudWithIntensities**，这是一个封装好的类。**ToPointCloudWithIntensities()是封装好的类。他是把ros**的消息类型转换成**cartographer**需要的消息类型。然后把时间戳，**pointCloud**送给\*\*HandleLaserScan()\*\*这个函数去处理。然后我们再看看这个函数。  
**sensor\_bridge.cc**

void SensorBridge::HandleLaserScan(
        const std::string& sensor_id, const carto::common::Time time,
        const std::string& frame_id,
        const carto::sensor::PointCloudWithIntensities& points) {
        
      if (points.points.empty()) {
          //如果点云为空，就return
        return;
      }
      CHECK_LE(points.points.back().time, 0.f);
      //把一帧点云分成好几份，为了处理点云的畸变
      // TODO(gaschler): Use per-point time instead of subdivisions.
      for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
        
        const size_t start_index =
            points.points.size() * i / num_subdivisions_per_laser_scan_;
        const size_t end_index =
            points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
        carto::sensor::TimedPointCloud subdivision(
            points.points.begin() + start_index, points.points.begin() + end_index);
        if (start_index == end_index) {
        
          continue;
        }
        const double time_to_subdivision_end = subdivision.back().time;
        // `subdivision_time` is the end of the measurement so sensor::Collator will
        // send all other sensor data first.
        const carto::common::Time subdivision_time =
            time + carto::common::FromSeconds(time_to_subdivision_end);
        auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
        if (it != sensor_to_previous_subdivision_time_.end() &&
            it->second >= subdivision_time) {
        
          LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                       << sensor_id << " because previous subdivision time "
                       << it->second << " is not before current subdivision time "
                       << subdivision_time;
          continue;
        }
        sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
        for (auto& point : subdivision) {
        
          point.time -= time_to_subdivision_end;
        }
        CHECK_EQ(subdivision.back().time, 0.f);
        HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
      }
    }

最重要的是这个`HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);` 将点云送到这里进行处理。  
**sensor\_bridge.cc**

void SensorBridge::HandleRangefinder(
        const std::string& sensor_id, const carto::common::Time time,
        const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
        
      if (!ranges.empty()) {
         //如果点云为空
        CHECK_LE(ranges.back().time, 0.f); //给每个点都赋予时间戳
      }
      const auto sensor_to_tracking =
          tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
      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>())});
      }
    }

其中**trajectory\_builder\_是map\_builder**中的**trajectory\_builder\_**,我们可以通过**map\_builder**的构造函数看一下。  
**collated\_tractory\_builder.h**

void AddSensorData(
          const std::string& sensor_id,
          const sensor::TimedPointCloudData& timed_point_cloud_data) override {
        
        AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
      }

两部分**sensor::MakeDispatchable**使它可以被分发，数据填充到**Dispatchable**这个类中  
**dispatchable.h**

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

将**data**封装到**Dispatchable**这个类中。除了数据本身，构造函数，还有**AddToTrajectoryBuilder()这个函数。  
sensor\_bridge把ros**类型转化为**cartographer**类型，然后再把数据变成类。变成类，再送给数据收集器，再去处理。  
**collator.cc**

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**里面。那么具体的**Add**的内容是什么呢，我们具体再看看。  
**ordered\_muti\_queue.cc**

void OrderedMultiQueue::Add(const QueueKey& queue_key,
                                std::unique_ptr<Data> data) {
        
      auto it = queues_.find(queue_key);
      if (it == queues_.end()) {
        
        LOG_EVERY_N(WARNING, 1000)
            << "Ignored data for queue: '" << queue_key << "'";
        return;
      }
      it->second.queue.Push(std::move(data));
      Dispatch();
    }

**OrderedMultiQueue**从字面意思是有序的多个队列，其实是数据同步收集器。加进来之后，做了一系列处理，然后进行分发。  
**ordered\_multi\_queue.cc**

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>();
          if (data == nullptr) {
        
            if (it->second.finished) {
        
              queues_.erase(it++);
              continue;
            }
            CannotMakeProgress(it->first);
            return;
          }
          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;
        }
        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.
        const common::Time common_start_time =
            GetCommonStartTime(next_queue_key.trajectory_id);
    
        if (next_data->GetTime() >= common_start_time) {
        
          // Happy case, we are beyond the 'common_start_time' already.
          last_dispatched_time_ = next_data->GetTime();
          next_queue->callback(next_queue->queue.Pop());
        } else if (next_queue->queue.Size() < 2) {
        
          if (!next_queue->finished) {
        
            // We cannot decide whether to drop or dispatch this yet.
            CannotMakeProgress(next_queue_key);
            return;
          }
          last_dispatched_time_ = next_data->GetTime();
          next_queue->callback(next_queue->queue.Pop());
        } 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.
          std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
          if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
        
            last_dispatched_time_ = next_data->GetTime();
            next_queue->callback(std::move(next_data_owner));
          }
        }
      }
    }

最终是怎么给出去的呢，经过同步的呢，这个Scan怎么给到外面的。

> next\_queue->callback(std::move(next\_data\_owner));

我们可以看到我们把队列的callback调用出来了。callback把同步了之后待分发的数据发布出去。还记的callback是什么吗？应该是一个拉姆达的表达式。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]  
下面就是真正的开始建图了。  
sensor\_bridge之前，早被轨迹构造器添加轨迹，collatedTrajectoryBuilder添加AddSensorData，同时把它MakeDispatchable,同时接收，处理完之后可以送给trajectory\_builder进行处理。AddData先进行数据同步，同步之后，找到相应队列的callback，同步之后，找到之前写好的拉姆达表达式。实际上送给HandleCollatedSensorData()去处理。最终data被送到trajectory\_builder轨迹构建器。实际上是global\_trajectory\_builder, 然后global\_trajectory\_builder拿到这个数据就可以真正的建图了。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]

[cartographer]: https://dqwang.blog.csdn.net/article/details/118539690
[cartographer 1]: https://dqwang.blog.csdn.net/article/details/118881867
[cartographer 2]: https://dqwang.blog.csdn.net/article/details/119023628
[cartographer 3]: https://dqwang.blog.csdn.net/article/details/119154163
[cartographer 4]: https://dqwang.blog.csdn.net/article/details/119845028
[cartographer 5]: https://dqwang.blog.csdn.net/article/details/120352115
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]: /images/20220829/bf5da8e8ea9d4cc1a3c775694fed6c6a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]: /images/20220829/f08d48c98c6a4d3f9062c67e2469eb4a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]: /images/20220829/66035a1f3e5b4ab3b6b68b5471cbfe57.png