C++?Cartographer源碼中關于傳感器的數(shù)據(jù)傳遞如何實現(xiàn)

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

進入Node::LaunchSubscribers看看

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

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

激光雷達,超聲雷達與點云數(shù)據(jù)處理與傳遞

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

  // laser_scan 的訂閱與注冊回調(diào)函數(shù), 多個laser_scan 的topic 共用同一個回調(diào)函數(shù)
  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的訂閱與注冊回調(diào)函數(shù)
  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

// 調(diào)用SensorBridge的傳感器處理函數(shù)進行數(shù)據(jù)處理
void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 根據(jù)配置,是否將傳感器數(shù)據(jù)跳過
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

注釋已經(jīng)寫了一部分內(nèi)容了,除此之外還調(diào)用了sensor_bridge的同名函數(shù)HandleLaserScanMessage, 看參數(shù)表示這個激光雷達的數(shù)據(jù)是某個軌跡id的某個sensorid的信息.

在進到這里看看. 在sensor_bridge.cc中實現(xiàn)

// 處理LaserScan數(shù)據(jù), 先轉(zhuǎn)成點云,再傳入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轉(zhuǎn)化成carto格式的PointCloudWithIntensities,

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

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

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

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

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

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

然后調(diào)用當前類的HandleRangefinder方法,處理分段后的點云.

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

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

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

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

里程計與IMU數(shù)據(jù)的走向

咱們看看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_使用里程計數(shù)據(jù)進行位姿預測
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

發(fā)現(xiàn)imu的數(shù)據(jù)走向有2個:

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

2. 傳入SensorBridge,使用其傳感器處理函數(shù)進行imu數(shù)據(jù)處理

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

在ToImuData方法中:

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

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

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

GPS與landmark數(shù)據(jù)走向

landmark從Node類中的回調(diào)函數(shù)進來,只有一個走向,直接傳入

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

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

  // 通過這個坐標變換 乘以 之后的gps數(shù)據(jù),就相當于減去了一個固定的坐標,從而得到了gps數(shù)據(jù)間的相對坐標變換
  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源碼中關于傳感器的數(shù)據(jù)傳遞如何實現(xiàn)”這篇文章的所有內(nèi)容，感謝各位的閱讀！相信大家閱讀完這篇文章都有很大的收獲，小編每天都會為大家更新不同的知識，如果還想學習更多的知識，請關注億速云行業(yè)資訊頻道。