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

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

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

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

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

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

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

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

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

以激光雷達(dá)為例, 進(jìn)入HandleLaserScanMessage

// 調(diào)用SensorBridge的傳感器處理函數(shù)進(jìn)行數(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ù)表示這個(gè)激光雷達(dá)的數(shù)據(jù)是某個(gè)軌跡id的某個(gè)sensorid的信息.

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

// 處理LaserScan數(shù)據(jù), 先轉(zhuǎn)成點(diǎ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>

返回值是一個(gè)打包好的std::tuple, 內(nèi)容是carto格式的點(diǎn)云和時(shí)間戳, 然后再由std::tie解壓成變量point_cloud和time, 傳入真正的數(shù)據(jù)處理函數(shù)- HandleLaserScan. 進(jìn)到這個(gè)函數(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)會(huì)一幀雷達(dá)數(shù)據(jù)被分成多少部分處理. 這個(gè)分塊函數(shù)還是挺有意思的, 學(xué)習(xí)一下自己以后也可以巧妙的均勻切分?jǐn)?shù)據(jù)了.

接下來就是檢查分割后點(diǎn)云是否有錯(cuò): 通過時(shí)間先后, 因?yàn)樯弦欢畏謮K的點(diǎn)云的末尾的一個(gè)點(diǎn)的時(shí)間戳不可能比這一塊點(diǎn)云的第一個(gè)點(diǎn)的時(shí)間戳還早,否則就是錯(cuò)的.

    if (it != sensor_to_previous_subdivision_time_.end() &&
        // 上一段點(diǎn)云的時(shí)間不應(yīng)該大于等于這一段點(diǎn)云的時(shí)間
        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;
    }
    // 更新對(duì)應(yīng)sensor_id的時(shí)間戳
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    // 檢查點(diǎn)云的時(shí)間
    for (auto& point : subdivision) {
      point.time -= time_to_subdivision_end;
    }
    CHECK_EQ(subdivision.back().time, 0.f);

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

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

TimedPointCloudData格式的點(diǎn)云, 然后才傳入到cartographer中.

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

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

里程計(jì)與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_使用里程計(jì)數(shù)據(jù)進(jìn)行位姿預(yù)測(cè)
  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個(gè):

1. 傳入PoseExtrapolator,用于位姿預(yù)測(cè)與重力方向的確定,

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

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

在ToImuData方法中:

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

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

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

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

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

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

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

  // 通過這個(gè)坐標(biāo)變換 乘以 之后的gps數(shù)據(jù),就相當(dāng)于減去了一個(gè)固定的坐標(biāo),從而得到了gps數(shù)據(jù)間的相對(duì)坐標(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算法部. 這一部分沒啥難點(diǎn).

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

您可能感興趣的文章: