Cartographer_ros

Cartographer_ros的数据处理与流向

cartographer数据类型

struct Subscriber {
    ::ros::Subscriber subscriber;

    // ::ros::Subscriber::getTopic() does not necessarily return the same
    // std::string
    // it was given in its constructor. Since we rely on the topic name as the
    // unique identifier of a subscriber, we remember it ourselves.
    std::string topic;
  };

enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    struct SensorId {
      SensorType type;  // 传感器的种类
      std::string id;   // topic的名字
    };

carto格式点云数据

//带时间戳的点的集合~可以理解为一帧点云数据了
struct PointCloudWithIntensities {
  std::vector<TimedRangefinderPoint> points;
  std::vector<float> intensities;
};

//带时间戳的点
struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};

carto格式时间：

struct UniversalTimeScaleClock {
  using rep = int64;
  using period = std::ratio<1, 10000000>;
  using duration = std::chrono::duration<rep, period>;
  using time_point = std::chrono::time_point<UniversalTimeScaleClock>;
  static constexpr bool is_steady = true;
};

using Duration = UniversalTimeScaleClock::duration; //单位是10e-7 s
using Time = UniversalTimeScaleClock::time_point;

node类启动slam

初始化：node_main.cc

Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

开启SLAM进程：

if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

调用node.cc

int Node::AddTrajectory(const TrajectoryOptions& options) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);
    const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);
  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);
  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);
  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

expected_sensor_ids传入前端点云时间同步， 在此处初始化

map_builder_bridge_.AddTrajectory(expected_sensor_ids, options)

开启一条轨迹，并初始化处理传感器数据的sensor_bridge

ros包的重点函数LaunchSubscribers

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  // laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
  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});
  }
  //以下重复操作
}

template <typename MessageType>  //模板函数，支持参数推演
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
               const typename MessageType::ConstPtr&), //函数指针的形参， 实际传入一个node类内的回调函数
    const int trajectory_id, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node) {

  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,  // kInfiniteSubscriberQueueSize = 0
      // 使用boost::function构造回调函数,被subscribe注册
      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表达式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);//函数指针的调用
          }));
}

SubscribeWithHandler函数实现ros::Subscriber的初始化，类似ros代码的demo写法。

接下来调用map_builder_bridge_类内数据成员sensor_bridge的成员方法进行传感器数据处理

主要是用来将ros格式转化为carto格式。map_builder_bridge_在node类构造时初始化，而数据成员sensor_bridge在调用map_builder_bridge_的AddTrajectory时初始化。

void Node::HandlePointCloud2Message(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::PointCloud2::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_); //互斥锁，保证当前数据不被修改
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return; //验证采样器，若数据被采样器抛弃，直接return
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandlePointCloud2Message(sensor_id, msg);
}

接下来处理ros格式的PointCloud2

// 处理ros格式的PointCloud2, 先转成点云,再传入trajectory_builder_
void SensorBridge::HandlePointCloud2Message(
    const std::string& sensor_id,
    const sensor_msgs::PointCloud2::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  //point_cloud最后一个点的时间戳为0，也就是point_cloud内点时间成员表示到最后一个点的时间间隔
 //点集的时间戳time为最后一个点的ICU时间，单位为10e-7s
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
}

ToPointCloudWithIntensities将ros格式的数据转换成carto格式，主要设计时间戳的变换，返回一个std::tuple类型，返回一个carto格式点集(每个点带一个时间戳)和一个一帧点云的时间戳

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
  PointCloudWithIntensities point_cloud;
  // We check for intensity field here to avoid run-time warnings if we pass in
  // a PointCloud2 without intensity.

  // 有强度数据
  if (PointCloud2HasField(msg, "intensity")) {
    // 有强度字段, 有时间字段
    if (PointCloud2HasField(msg, "time")) {
      pcl::PointCloud<PointXYZIT> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
        point_cloud.intensities.push_back(point.intensity);
      }
    }
    // 有强度字段, 没时间字段
    else {
      pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z},
             0.f});  // 没有时间信息就把时间填0
        point_cloud.intensities.push_back(point.intensity);
      }
    }
  }
  // 没有强度数据
  else {
    // If we don't have an intensity field, just copy XYZ and fill in 1.0f.
    // 没强度字段, 有时间字段
    if (PointCloud2HasField(msg, "time")) {
      pcl::PointCloud<PointXYZT> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
        point_cloud.intensities.push_back(1.0f);
      }
    }
    // 没强度字段, 没时间字段
    else {
      pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z},
             0.f});  // 没有时间信息就把时间填0
        point_cloud.intensities.push_back(1.0f);
      }
    }
  }

  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
  if (!point_cloud.points.empty()) {
    const double duration = point_cloud.points.back().time;
    // 点云开始的时间 加上 第一个点到最后一个点的时间
    // 点云最后一个点的时间 作为整个点云的时间戳
    timestamp += cartographer::common::FromSeconds(duration);

    for (auto& point : point_cloud.points) {
      // 将每个点的时间减去整个点云的时间, 所以每个点的时间都应该小于0
      point.time -= duration;

      // 对每个点进行时间检查, 看是否有数据点的时间大于0, 大于0就报错
      CHECK_LE(point.time, 0.f)
          << "Encountered a point with a larger stamp than "
             "the last point in the cloud.";
    }
  }
  return std::make_tuple(point_cloud, timestamp);
}

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

  // 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin
  // 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_
  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>())} ); // 强度始终为空
  }
}