C++?Cartographer源碼中關于傳感器的數據傳遞如何實現

今天小編給大家分享一下C++ Cartographer源碼中關于傳感器的數據傳遞如何實現的相關知識點，內容詳細，邏輯清晰，相信大部分人都還太了解這方面的知識，所以分享這篇文章給大家參考一下，希望大家閱讀完這篇文章后有所收獲，下面我們一起來了解一下吧。

進入Node::LaunchSubscribers看看

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id);

可以看到, 傳入的參數只有當前軌跡id和配置參數. 咱們看看激光雷達和超聲雷達的內容

激光雷達,超聲雷達與點云數據處理與傳遞

存在3中雷達數據, 一種是單線雷達點云(LaserScan), 一種是多回聲波雷達點云(MultiEchoLaserScanMessage),一種是點云(PointCloud2)可以處理多線雷達,

  // 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});
  }
  // multi_echo_laser_scans的訂閱與注冊回調函數
  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});
  }

以激光雷達為例, 進入HandleLaserScanMessage

// 調用SensorBridge的傳感器處理函數進行數據處理
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_bridge的同名函數HandleLaserScanMessage, 看參數表示這個激光雷達的數據是某個軌跡id的某個sensorid的信息.

在進到這里看看. 在sensor_bridge.cc中實現

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

ToPointCloudWithIntensities把ros格式的LaserScan轉化成carto格式的PointCloudWithIntensities,

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>

返回值是一個打包好的std::tuple, 內容是carto格式的點云和時間戳, 然后再由std::tie解壓成變量point_cloud和time, 傳入真正的數據處理函數- HandleLaserScan. 進到這個函數再看看

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

這一塊標會一幀雷達數據被分成多少部分處理. 這個分塊函數還是挺有意思的, 學習一下自己以后也可以巧妙的均勻切分數據了.

接下來就是檢查分割后點云是否有錯: 通過時間先后, 因為上一段分塊的點云的末尾的一個點的時間戳不可能比這一塊點云的第一個點的時間戳還早,否則就是錯的.

    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_id的時間戳
    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方法,處理分段后的點云.

HandleRangefinder() 函數將點云點云從雷達坐標系下轉到tracking_frame坐標系系下, 并轉成

TimedPointCloudData格式的點云, 然后才傳入到cartographer中.

值得注意的是,不論是激光雷達還是超聲雷達,最終都調用的是SensorBridge::HandleLaserScan.

咱們再看看最后一種激光傳感器類型-點云,不同于激光雷達和超聲雷達, 這個Cartographer定義的傳感器類型接受直接的點云數據, 所以免去了數據分割等數據處理. 他調用了SensorBridge::HandlePointCloud2Message這個方法, 這個方法同樣調用了HandleRangefinder. 所以HandleRangefinder這個函數實現了各種雷達與點云傳感器類型的統一.可以看到調用關系上點云少了一步.

里程計與IMU數據的走向

咱們看看Node::HandleImuMessage,

void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  // extrapolators_使用里程計數據進行位姿預測
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

發現imu的數據走向有2個:

1. 傳入PoseExtrapolator,用于位姿預測與重力方向的確定,

2. 傳入SensorBridge,使用其傳感器處理函數進行imu數據處理

IMU數據通過sensor_bridge的ToImuData轉化為了位姿推測器的imudata, 然后傳遞給了位姿推測器的AddImuData, 用于位姿預測. 同樣, HandleImuMessage也采用了ToImuData轉化了imu的data, 然后調用trajectory_builder_的AddSensorData添加數據到軌跡中, 實現定位.

在ToImuData方法中:

const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->header.frame_id));

這個是通過tf得到傳感器相對于機器人坐標系的轉換關系, 由于imu的hz是很高的(1000hz左右), 所以機器人一般以IMU所在位子為機器人的坐標, 以減小計算量.

里程計和IMU數據是一樣的, 都是兩個數據走向, 參照IMU.

GPS與landmark數據走向

landmark從Node類中的回調函數進來,只有一個走向,直接傳入

SensorBridge::HandleLandmarkMessage()函數中,通過tf轉換到機器人坐標系下后,再從SensorBridge傳遞給cartographer.

GPS數據和landmark數據一樣, 只有一個走向, 和landmark不同的是, GPS數據需要進行坐標轉換,因為GPS得到的raw data是lla坐標, 需要轉化為ECEF坐標, 然后計算兩幀GPS數據之間的相對坐標轉換. 程序如下:

  // 通過這個坐標變換 乘以 之后的gps數據,就相當于減去了一個固定的坐標,從而得到了gps數據間的相對坐標變換
  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)))});

以上就是“C++ Cartographer源碼中關于傳感器的數據傳遞如何實現”這篇文章的所有內容，感謝各位的閱讀！相信大家閱讀完這篇文章都有很大的收獲，小編每天都會為大家更新不同的知識，如果還想學習更多的知識，請關注億速云行業資訊頻道。