cartographer源码解析（八）雷达数据

绿竹巷人

分类：ROS

发布时间 2021.07.01阅读数 5040 评论数 0

描述

主要分析cartographer处理雷达数据的代码

在代码中cout来理解

1. HandleLaserScanMessage

cartographer_ros/node.cc中的HandleLaserScanMessage()函数中，我添加了如下几个cout

std::cout<<"sensor_id: "<<sensor_id<<" time: "<<time<<" frame_id: "<<frame_id<<std::endl;
std::cout<<"num_subdivisions_per_laser_scan_: "<<num_subdivisions_per_laser_scan_<<std::endl;
std::cout<<"points.points.size() : "<<points.points.size() <<std::endl;
std::cout<<"points.points.back().time : "<<points.points.back().time <<std::endl;
std::cout<<"points.points.back().position : "<<points.points.back().position <<std::endl;
std::cout<<"points.points[1078].position : "<<points.points[1078].position <<std::endl;
std::cout<<"points.intensities[0] : "<<points.intensities[0] <<std::endl;
std::cout<<"points.points[500].position.x : "<<points.points[500].position[0] <<std::endl;
std::cout<<"points.points[500].position.y : "<<points.points[500].position[1] <<std::endl;
std::cout<<"points.points[500].position.z : "<<points.points[500].position[2] <<std::endl;

HandleLaserScanMessage()函数会被持续调用，因此我的打印也会持续输出，我随便截取两段放在下面

sensor_id: echoes time: 635682438189920596 frame_id: horizontal_laser_link
num_subdivisions_per_laser_scan_: 10
points.points.size() : 1079
points.points.back().time : 0
points.points.back().position : -2.14016
 2.15901
       0
points.points[1078].position : -2.14016
 2.15901
       0
points.intensities[0] : 3019
points.points[500].position.x : 0.67609
points.points[500].position.y : -0.116181
points.points[500].position.z : 0

------------------ 分割线，为了两段信息看起来更清楚 -----------------------------

sensor_id: echoes time: 635682438190186686 frame_id: horizontal_laser_link
num_subdivisions_per_laser_scan_: 10
points.points.size() : 1079
points.points.back().time : 0
points.points.back().position : -2.10426
 2.12279
       0
points.points[1078].position : -2.10426
 2.12279
       0
points.intensities[0] : 3001
points.points[500].position.x : 0.682004
points.points[500].position.y : -0.117197
points.points[500].position.z : 0

变量解释

sensor_id、time、frame_id  // 代表着每一帧雷达数据的属性
num_subdivisions_per_laser_scan_ // 每帧雷达被划分成10份
points.points.size // 每帧雷达有1079个点
points.intensities[0] // 当前雷达帧的强度
points.points.back().position // 每帧雷达最后一个点的三维x、y、z值
points.points[1078].position // 每帧雷达第1078个点的三维x、y、z值，所以也就是最后一个点
points.points[500].position.x、y、z // 第500个点的x、y、z值

points的类型定义在cartographer/cartographer/sensor/point_cloud.h中

using TimedPointCloud = std::vector<TimedRangefinderPoint>;
struct PointCloudWithIntensities {
  TimedPointCloud points;
  std::vector<float> intensities;
};

points.points的类型就是TimedPointCloud，也就是一个vector的TimedRangefinderPoint，定义在cartographer/cartographer/sensor/point_cloud.h

struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};

Eigen::Vector3f是一个三维向量，这样你应该能完全看懂我的cout了

雷达数据时间
另外，打印1079个点的time会发现，第0个点的time是-0.0186806，第1078个点的time是0，中间的点的time从-0.018依次增加直到为0

不难猜测，每一帧的点云的时间是以最后一个雷达数据点的时间为准，以我的过往经验雷达中间的转子是逆时针旋转的，第0个点在-135度，第1078个点在135度。因此第0个点是最先扫描到的，时间最小；第1078个点是最后扫描到的，时间最大。

有兴趣的话你还可以拿雷达的position去验证，打印0~1078个点的position，你会发现它的数值是以雷达为中心的，不同扫描角度时的雷达数据position值的正负，是符合雷达坐标系的。

雷达数据分段
变量中有一个这样的参数

num_subdivisions_per_laser_scan_ // 每帧雷达被划分成10份

我们可以在代码中再次cout

std::cout<<" start_index: "<<start_index<<" end_index: "<<end_index<<std::endl;

会发现输出是这样的

start_index: 0 end_index: 107
start_index: 107 end_index: 215
start_index: 215 end_index: 323
start_index: 323 end_index: 431
start_index: 431 end_index: 539
start_index: 539 end_index: 647
start_index: 647 end_index: 755
start_index: 755 end_index: 863
start_index: 863 end_index: 971
start_index: 971 end_index: 1079

显然雷达数据被分成了10份

源码层次

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

核心语句

map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);

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

核心语句是

HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);

cartographer_ros/sensor_bridge.cc

HandleLaserScan仍然定义在cartographer_ros/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;
  }
  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);

cartographer_ros/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_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, sensor_to_tracking->translation().cast<float>(),
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())});

cartographer/mapping/internal/collated_trajectory_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));
  }

cartographer/mapping/internal/collated_trajectory_builder.cc

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

cartographer/sensor/internal/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));
}

cartographer/sensor/internal/ordered_multi_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();
}

cartographer/sensor/internal/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));
      }
    }
  }
}

ROS SLAM 建图导航 Cartographer 源码解析

打赏 0

上一篇：Mac 编译报错 Undefined symbols for architecture x86_64

下一篇：Cmake工程报warning: XXX is deprecated [-Wdeprecated-declarations]

cartographer源码解析（八）雷达数据

绿竹巷人

描述

1. HandleLaserScanMessage

源码层次

为你推荐

permission denied出现时，不妨尝试加sudo

ROS学习（21）机器人SLAM功能包——orbslam的安装与测试

ros机器人编程实践（7）- turtlebot3仿真与脚本控制！！！史上最全，入门最简

Airsim+Lego-Loam

快速使用ros小乌龟教程——ROS初体验

在分布式多机通讯过程中使用ros_arduino_bridge功能包查看rqt_tf_tree出错：PluginHandlerDirect._restore_settings() plugin....

评论（0）

关于作者

绿竹巷人

89

0

26

5

Python使用Eigen库的一些操作

Mac 编译报错 Undefined symbols for architecture x86_64

2022年最新成果——Patchwork++地面分割

相关推荐

手把手教你用Gazebo仿真UUV水下机器人

rosjava发布者的使用

ROS系统学习6---摄像头图像数据的采集

ROS操作系统学习（四）创建工作空间与功能包

ROS暑期学校8月10日知识列表在这里，抽奖运气在你手里～

跨ROS系统通信：使用TCP实现节点间的直连

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

DeepLearning

经典算法

自动驾驶

移动机器人—下位机开发

计算机视觉

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板