cartographer源码分析（4）sensor_bridge.cc以及sensor_bridge.h

包含了：

1、里程计的预处理函数

2、GPS的预处理函数

3、IMU的预处理函数

4、点云的预处理函数

以上函数都调用了AddSensorData函数。具体的数据流程：传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的StartTrajectory这个服务会订阅传感器数据---->接收到该数据由相应的处理函数处理，

比如Node::HandleImuMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridge变量进行处理---->调用了TrajectoryBuilder的

虚函数AddSensorData()---->CollatedTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数

sensor_bridge.cc的注释

#include "cartographer_ros/sensor_bridge.h"

#include "absl/memory/memory.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"

namespace cartographer_ros {

namespace carto = ::cartographer;

using carto::transform::Rigid3d;

namespace {

const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
  if (frame_id.size() > 0) {
    CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
                               << " should not start with a /. See 1.7 in "
                                  "http://wiki.ros.org/tf2/Migration.";
  }
  return frame_id;
}

}  // namespace
//构造函数做的工作就是把参数表赋值给成员函数
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) {}
//处理里程计的函数
//一个预处理的工具函数，并非SensorBridge的成员变量；
//其参数类型是nav_msgs::Odometry::ConstPtr&,
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));//该函数返回的是一个变换矩阵，查询的是某时刻某一帧数据的变换估计。估计要用来做累加
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  return absl::make_unique<carto::sensor::OdometryData>(
      carto::sensor::OdometryData{
          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

void SensorBridge::HandleOdometryMessage(
    const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
      ToOdometryData(msg);
  if (odometry_data != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
  }
}
//这个需要注意的是TfBridge这个类，他的实例化对象是tf_bridge_，tf_bridge_是SensorBridge的一个成员变量。我们通过查看TfBridge可以看到，作者在设计的时候，
//通过TfBridge把不同传感器的差异进行了一下屏蔽，不管什么传感器，都会对每一帧数据的位姿进行估计，而这个历史的位姿数据就以TfBridge的形式存起来。
//这样在使用的时候也可以通过TfBridge查询某一个传感器在某一个历史时刻的位姿。
//这里同样也是，代码通过TfBridge查询了一下历史数据，然后把这个作为参数传给了TrajectoryBuilder的AddSensorData来做处理。前面我们已经介绍过了，
//TrajectoryBuilder也是为不同的传感器提供了统一的处理接口。我们可以在TrajectoryBuilder的具体实现里再看具体做了什么操作。
//但我们可以合理猜测，比如，里程计的数据是在原来数据的基础上再做一个累加，作为新的值同样保存到TfBridge里。
//以里程计数据为例，我们可以梳理一下传感器数据整个的处理流程：
//传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的StartTrajectory这个服务会订阅传感器数据---->接收到该数据由相应的处理函数处理，
//比如Node::HandleImuMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridge变量进行处理---->调用了TrajectoryBuilder的
//虚函数AddSensorData()---->CollatedTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数



//可能是GPS的数据，返回的是在世界坐标系下的坐标x,y,z。但是对于室内机器人来说，GPS无法应用，GPS信号对于室外机器人会更有用处。
//cartographer在提供的官方测试数据里，也是没有使用GPS数据的（见/src/cartographer_ros/configuration_files/backpack_2d.lua中设置use_nav_sat = false）。
void SensorBridge::HandleNavSatFixMessage(
    const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});
    return;
  }

  if (!ecef_to_local_frame_.has_value()) {
    ecef_to_local_frame_ =
        ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
    LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = "
              << msg->latitude << ", long = " << msg->longitude << ".";
  }

  trajectory_builder_->AddSensorData(
      sensor_id, carto::sensor::FixedFramePoseData{
                     time, absl::optional<Rigid3d>(Rigid3d::Translation(
                               ecef_to_local_frame_.value() *
                               LatLongAltToEcef(msg->latitude, msg->longitude,
                                                msg->altitude)))});
}
// 处理Landmark
void SensorBridge::HandleLandmarkMessage(
    const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
  auto landmark_data = ToLandmarkData(*msg);

  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
  if (tracking_from_landmark_sensor != nullptr) {
    for (auto& observation : landmark_data.landmark_observations) {
      observation.landmark_to_tracking_transform =
          *tracking_from_landmark_sensor *
          observation.landmark_to_tracking_transform;
    }
  }
  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}
//处理IMU数据
//同样，数据预处理函数。并非SensorBridge的成员函数
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
    const sensor_msgs::Imu::ConstPtr& msg) {
		//确保IMU工作正常
  CHECK_NE(msg->linear_acceleration_covariance[0], -1)
      << "Your IMU data claims to not contain linear acceleration measurements "
         "by setting linear_acceleration_covariance[0] to -1. Cartographer "
         "requires this data to work. See "
         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
  CHECK_NE(msg->angular_velocity_covariance[0], -1)
      << "Your IMU data claims to not contain angular velocity measurements "
         "by setting angular_velocity_covariance[0] to -1. Cartographer "
         "requires this data to work. See "
         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";

  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->header.frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
      << "The IMU frame must be colocated with the tracking frame. "
         "Transforming linear acceleration into the tracking frame will "
         "otherwise be imprecise.";
  return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
      time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
      sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}
//最终，将先加速度和角加速度传入trajectory_builder_->AddSensorData做处理
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                    const sensor_msgs::Imu::ConstPtr& msg) {
  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
  if (imu_data != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});
  }
}
//调用了SensorBridge::HandleLaserScan来做处理。
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);
}
//分析node对象的时候我们已经看到，node对象在激光传感器的消息回调函数中， 通过轨迹索引trajectory_id从map_builder_bridge_对象中查询获得对应轨迹的SensorBridge对象，
//并通过该对象的HandleMultiEchoLaserScanMessage函数来处理雷达数据。 下面是该函数的代码片段，前两行只是定义了两个临时变量分别用于记录转换之后的点云数据和时间戳。 
//先在第4行中通过msg_conversion.h, msg_conversion.cc中定义和实现的函数ToPointCloudWithIntensities， 将ROS的消息转换成点云数据。
//然后在第5行中通过成员函数HandleLaserScan来处理点云数据。
//调用了SensorBridge::HandleLaserScan来做处理。
//函数ToPointCloudWithIntensities所做的工作就是根据ROS的消息内容，计算扫描到的障碍物在工作空间中的坐标位置，并将其保存在一个特定的数据结构中。
void SensorBridge::HandleMultiEchoLaserScanMessage(
    const std::string& sensor_id,
    const sensor_msgs::MultiEchoLaserScan::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);
}
//也是调用了SensorBridge::HandleRangefinder处理。这种设计使得重复性比较大的SensorBridge::HandleRangefinder中的代码得以复用。
void SensorBridge::HandlePointCloud2Message(
    const std::string& sensor_id,
    const sensor_msgs::PointCloud2::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
}

const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
//分析函数HandleMultiEchoLaserScanMessage的时候我们看到，将ROS的消息数据转换成为Cartographer的点云数据之后， 就调用函数HandleLaserScan来将处理后的数据
//传递给Cartographer进行后序的处理了。HandleLaserScan的代码片段，在函数的一开始先确认一下输入的点云非空， 并且最后一个点云数据的时间小于等于0。
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.
  //然后根据配置变量num_subdivisions_per_laser_scan_在一个for循环中将点云数据拆分为若干段。第6和第7行计算分段的起始和结束索引，在第8行中构建分段数据。
  //第9,10行是为了跳过同一个分段中的元素。
  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;
    }
	//接着参考分段中最后一个数据的时间调整其他数据的时间，但在该操作之前需要先确认当前的数据没有过时。成员容器sensor_to_previous_subdivision_time_中
	//以sensor_id为索引记录了最新的数据产生时间， 如果分段的时间落后于记录值，将抛弃所对应的数据。
    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将分段数据喂给Cartographer了。
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  }
}



//处理激光、多激光、点云数据
//这几个函数初看似乎没有调用trajectory-->AddSensorData函数。但仔细研究会发现他们之间彼此存在调用关系。
//最根上是HandleRangefinder这个函数：这个函数调用了trajectory_builder_->AddSensorData来处理

//而之后，激光数据函数SensorBridge::HandleLaserScan调用了SensorBridge::HandleRangefinder来做处理。所以这里相当于做了一层抽象。
//我们的Rangefinder可以不一定是激光，也可以是其他类型的传感器，比如Kinect。这样，以后如果要扩展或修改，我们可以不改之前的代码，而只需要多写一个处理Kinect的代码就可以。
//这也是封装的好处。所有做的这些复杂的工作，都是为了程序员维护的方便，但对于读别人代码的人来说，要理清作者的设计思路，那可就需要费一番功夫了。

//在该函数中先通过tf_bridge_对象查询传感器坐标系相对于机器人坐标系之间的坐标变换，记录在对象sensor_to_tracking中。 只要该对象不是空指针，
//就说明成功找到转换关系，然后就可以通过对象trajectory_builder_添加传感器数据了。 这个trajectory_builder_实际上就是由map_builder对象提供的一个对象。
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>())});
  }
}

}  // namespace cartographer_ros

主要参考：

https://zhuanlan.zhihu.com/p/48296627

https://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=%E4%BD%BF%E7%94%A8SensorBridge%E8%BD%AC%E6%8D%A2%E6%BF%80%E5%85%89%E4%BC%A0%E6%84%9F%E5%99%A8%E6%95%B0%E6%8D%AE