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

更新時間：2023年03月31日 08:41:40 作者：蝦眠不覺曉,

這篇文章主要介紹了C++?Cartographer源碼中關(guān)于傳感器的數(shù)據(jù)傳遞實現(xiàn)，前面已經(jīng)談到了Cartographer中添加軌跡的方法和傳感器的數(shù)據(jù)流動走向。發(fā)現(xiàn)在此調(diào)用了LaunchSubscribers這個函數(shù)來訂閱相關(guān)傳感器數(shù)據(jù)

上一節(jié)我們看了添加軌跡的相關(guān)處理，發(fā)現(xiàn)在此調(diào)用了LaunchSubscribers這個函數(shù)來訂閱相關(guān)傳感器數(shù)據(jù). 這一節(jié)看看這些訂閱的傳感器數(shù)據(jù)的流動過程. 咱們進入Node::LaunchSubscribers看看

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

可以看到, 傳入的參數(shù)只有當(dāng)前軌跡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_;

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

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

    if (it != sensor_to_previous_subdivision_time_.end() &&
        // 上一段點云的時間不應(yīng)該大于等于這一段點云的時間
        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;
    }
    // 更新對應(yīng)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)用當(dāng)前類的HandleRangefinder方法,處理分段后的點云.

HandleRangefinder() 函數(shù)將點云點云從雷達坐標(biāo)系下轉(zhuǎn)到tracking_frame坐標(biāo)系系下, 并轉(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)用關(guān)系上點云少了一步.

里程計與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ù)進行位姿預(yù)測
  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,用于位姿預(yù)測與重力方向的確定,

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

IMU數(shù)據(jù)通過sensor_bridge的ToImuData轉(zhuǎn)化為了位姿推測器的imudata, 然后傳遞給了位姿推測器的AddImuData, 用于位姿預(yù)測. 同樣, 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得到傳感器相對于機器人坐標(biāo)系的轉(zhuǎn)換關(guān)系, 由于imu的hz是很高的(1000hz左右), 所以機器人一般以IMU所在位子為機器人的坐標(biāo), 以減小計算量.

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

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

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

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

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

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

總結(jié)

綜上所述, 所有的數(shù)據(jù)都要通過處理轉(zhuǎn)化為Cartographer能夠接受的數(shù)據(jù)結(jié)構(gòu), 然后通過調(diào)用trajectory_builder_的addsensordata方法, 把傳感器數(shù)據(jù)給到Cartographer算法部. 這一部分沒啥難點.

到此這篇關(guān)于C++ Cartographer源碼中關(guān)于傳感器的數(shù)據(jù)傳遞實現(xiàn)的文章就介紹到這了,更多相關(guān)C++ Cartographer數(shù)據(jù)傳遞內(nèi)容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！

您可能感興趣的文章: