C++Node類Cartographer開始軌跡的處理深度詳解

更新時(shí)間：2023年03月16日 11:42:03 作者：蝦眠不覺曉,

這篇文章主要介紹了C++Node類Cartographer開始軌跡的處理，文中通過示例代碼介紹的非常詳細(xì)，對大家的學(xué)習(xí)或者工作具有一定的參考學(xué)習(xí)價(jià)值，需要的朋友們下面隨著小編來一起學(xué)習(xí)吧

上一節(jié)我們看了一下node_main.cc的內(nèi)容,發(fā)現(xiàn)其中最重要的部分就是Node類,這個(gè)類吃下了map_builder類,且里面實(shí)現(xiàn)了傳感器數(shù)據(jù)的處理與分發(fā)(sensor_bridge),還有整個(gè)Cartographer算法的調(diào)用(map_builder與map_builder_bridge). 這一節(jié)我們重點(diǎn)看一下Node類的初始化和開始軌跡部分到底做了些什么.

Node.h頭文件

在Node.h中,包含了以下幾個(gè)部分(程序太大,就不貼了):

構(gòu)造,析構(gòu),拷貝構(gòu)造,賦值函數(shù)的構(gòu)建

Node(const NodeOptions& node_options,
       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
  ~Node();
  // c++11: =delete: 禁止編譯器自動(dòng)生成默認(rèn)函數(shù); =default: 要求編譯器生成一個(gè)默認(rèn)函數(shù)
  // 禁止編譯器自動(dòng)生成 默認(rèn)拷貝構(gòu)造函數(shù)(復(fù)制構(gòu)造函數(shù))
  Node(const Node&) = delete;
  // 禁止編譯器自動(dòng)生成 默認(rèn)賦值函數(shù)
  Node& operator=(const Node&) = delete;

軌跡有關(guān)部分

  // Finishes all yet active trajectories.
  void FinishAllTrajectories();
  // Finishes a single given trajectory. Returns false if the trajectory did not
  // exist or was already finished.
  bool FinishTrajectory(int trajectory_id);
  // Runs final optimization. All trajectories have to be finished when calling.
  void RunFinalOptimization();
  // Starts the first trajectory with the default topics.
  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);

傳感器數(shù)據(jù)部分

  // The following functions handle adding sensor data to a trajectory.
  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
                             const nav_msgs::Odometry::ConstPtr& msg);
  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
                              const sensor_msgs::NavSatFix::ConstPtr& msg);
  void HandleLandmarkMessage(
      int trajectory_id, const std::string& sensor_id,
      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
  void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
                        const sensor_msgs::Imu::ConstPtr& msg);
  void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
                              const sensor_msgs::LaserScan::ConstPtr& msg);
  void HandleMultiEchoLaserScanMessage(
      int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
  void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
                                const sensor_msgs::PointCloud2::ConstPtr& msg);

其他部分.

比如傳感器采樣設(shè)置,位姿推測器等部分.

Node類的構(gòu)造函數(shù)

/**
 * @brief
 * 聲明ROS的一些topic的發(fā)布器, 服務(wù)的發(fā)布器, 以及將時(shí)間驅(qū)動(dòng)的函數(shù)與定時(shí)器進(jìn)行綁定
 *
 * @param[in] node_options 配置文件的內(nèi)容
 * @param[in] map_builder SLAM算法的具體實(shí)現(xiàn)
 * @param[in] tf_buffer tf
 * @param[in] collect_metrics 是否啟用metrics,默認(rèn)不啟用
 */
Node::Node(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
  // 將mutex_上鎖, 防止在初始化時(shí)數(shù)據(jù)被更改
  absl::MutexLock lock(&mutex_);
  // 默認(rèn)不啟用
  if (collect_metrics) {
    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
  }
  // Step: 1 聲明需要發(fā)布的topic
  // 發(fā)布SubmapList
  submap_list_publisher_ =
      node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
          kSubmapListTopic, kLatestOnlyPublisherQueueSize);
  // 發(fā)布軌跡
  trajectory_node_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
  // 發(fā)布landmark_pose
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
  // 發(fā)布約束
  constraint_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kConstraintListTopic, kLatestOnlyPublisherQueueSize);
  // 發(fā)布tracked_pose, 默認(rèn)不發(fā)布
  if (node_options_.publish_tracked_pose) {
    tracked_pose_publisher_ =
        node_handle_.advertise<::geometry_msgs::PoseStamped>(
            kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
  }
  // lx add
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    point_cloud_map_publisher_ =
        node_handle_.advertise<sensor_msgs::PointCloud2>(
            kPointCloudMapTopic, kLatestOnlyPublisherQueueSize, true);
  }
  // Step: 2 聲明發(fā)布對應(yīng)名字的ROS服務(wù), 并將服務(wù)的發(fā)布器放入到vector容器中
...
  // Step: 3 處理之后的點(diǎn)云的發(fā)布器
...
  // Step: 4 進(jìn)行定時(shí)器與函數(shù)的綁定, 定時(shí)發(fā)布數(shù)據(jù)
...
  // lx add
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    wall_timers_.push_back(node_handle_.createWallTimer(
        ::ros::WallDuration(kPointCloudMapPublishPeriodSec),  // 10s
        &Node::PublishPointCloudMap, this));
  }
}

Node構(gòu)造函數(shù)最重要的兩個(gè)傳入變量是node_options和map_builder. 這兩個(gè)在上一節(jié)中已經(jīng)詳細(xì)說過了,map_builder是Cartographer算法部分,包含了前端和后端.

構(gòu)造函數(shù)通過使用初始化列表去初始化一些私有變量(node_options_, map_builder_bridge_), 然后使用MutexLock上鎖. 初始化列表和智能鎖比較基礎(chǔ)就不詳細(xì)介紹了.

總之,這個(gè)構(gòu)造函數(shù)使用node_options初始化了map_builder_bridge, 而map_builder_bridge又調(diào)用Cartographer算法部分的map_builder(前,后端), 同時(shí)還確定了要發(fā)布的topic,和可視化所需的topic.

開始一條軌跡

現(xiàn)在介紹一下Node::AddTrajectory。這塊函數(shù)是這一節(jié)的重中之重了,是node.cc中的核心部分. 它維護(hù)了傳感器列表, 添加了一條軌跡,新增了一個(gè)位姿估計(jì)器,傳感器數(shù)據(jù)采樣器,還有訂閱所需的topic和注冊對應(yīng)的回調(diào)函數(shù). 為了確保topic沒有重復(fù),他還保存了注冊topic的鍵值對,以供查詢是否重復(fù).

添加一條軌跡

添加軌跡的函數(shù)是AddTrajectory

/**
 * @brief 添加一個(gè)新的軌跡
 *
 * @param[in] options 軌跡的參數(shù)配置
 * @return int 新生成的軌跡的id
 */
int Node::AddTrajectory(const TrajectoryOptions& options);

添加傳感器維護(hù)功能:

調(diào)用了Node的函數(shù)ComputeExpectedSensorIds, 作用是根據(jù)配置文件,去返回一個(gè)傳感器列表:

std::set<SensorId> expected_topics;

在看傳感器列表之前咱們先看一下Cartographer中傳感器類型的定義:

    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };
    struct SensorId {
      SensorType type;  // 傳感器的種類
      std::string id;   // topic的名字
      ......
    };

它規(guī)定了一個(gè)傳感器的類型與一個(gè)對應(yīng)的topic的名字. 傳感器的類型是一個(gè)限域枚舉(枚舉類). 總之作用就是聯(lián)系topic與topic對應(yīng)的傳感器類型, 以便后續(xù)維護(hù).

回到之前的代碼,不難看出, 一個(gè)軌跡的傳感器的列表有一下命名規(guī)則:

如果只有一個(gè)傳感器, 那訂閱的topic就是topic

如果是多個(gè)傳感器, 那訂閱的topic就是topic_1,topic_2, 依次類推(多個(gè)超聲雷達(dá))

3d slam必須有imu, 2d可有可無, imu的topic的個(gè)數(shù)只能有一個(gè)

里程計(jì)可有可無, topic的個(gè)數(shù)只能有一個(gè)

gps可有可無, topic的個(gè)數(shù)只能有一個(gè)

Landmark可有可無, topic的個(gè)數(shù)只能有一個(gè)

添加一個(gè)軌跡

接下來就是最重要的函數(shù), AddTrajectory, Cartographer的核心, 傳感器數(shù)據(jù)和Cartographer算法庫連接處的大門, 調(diào)用了ros部分的map_builder_bridge和算法部分的map_builder產(chǎn)生聯(lián)系. 聯(lián)系的實(shí)現(xiàn)方法相當(dāng)復(fù)雜, 將在另一節(jié)詳細(xì)說

  // 調(diào)用map_builder_bridge的AddTrajectory, 添加一個(gè)軌跡
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

這一行調(diào)用了Map_builder_bridge_的AddTrajectory添加一條軌跡. 傳入的參數(shù)有一個(gè)std::set<...Sensor_id>類型的變量,std::set是一個(gè)容器,可以簡單理解為鍵值對,而鍵就是值,值就是鍵.(比較基礎(chǔ)不細(xì)說啦).另一個(gè)就是從node_main.cc就跟著我們的TrajectoryOptions. 也就是配置文件讀取的內(nèi)容. 返回值很簡單,就是新建軌跡的編號. Cartographer允許有多個(gè)軌跡同時(shí)維護(hù),而且后面我們會發(fā)現(xiàn), Cartographer定位其實(shí)就是把建好的地圖和定位作為兩個(gè)不同的軌跡實(shí)現(xiàn)的. 這個(gè)函數(shù)是整個(gè)Cartographer的功能實(shí)現(xiàn), 方法很復(fù)雜, 將會在以后MapBuilder部分詳細(xì)說.

新增位姿估計(jì)器

這個(gè)函數(shù)功能是通過IMU和里程計(jì)(輪編碼器)去預(yù)估下一次可能的位姿,給定位一個(gè)初始值

/**
 * @brief 新增一個(gè)位姿估計(jì)器
 * 
 * @param[in] trajectory_id 軌跡id
 * @param[in] options 參數(shù)配置
 */
void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
  // 新生成的軌跡的id 不應(yīng)該在extrapolators_中
  CHECK(extrapolators_.count(trajectory_id) == 0);
  // imu_gravity_time_constant在2d, 3d中都是10
  const double gravity_time_constant =
      node_options_.map_builder_options.use_trajectory_builder_3d()
          ? options.trajectory_builder_options.trajectory_builder_3d_options()
                .imu_gravity_time_constant()
          : options.trajectory_builder_options.trajectory_builder_2d_options()
                .imu_gravity_time_constant();
  // c++11: map::emplace() 用于通過在容器中插入新元素來擴(kuò)展map容器
  // 元素是直接構(gòu)建的（既不復(fù)制也不移動(dòng)）.僅當(dāng)鍵不存在時(shí)才進(jìn)行插入
  // c++11: std::forward_as_tuple tuple的完美轉(zhuǎn)發(fā)
  // 該 tuple 在以右值為參數(shù)時(shí)擁有右值引用數(shù)據(jù)成員, 否則擁有左值引用數(shù)據(jù)成員
  // c++11: std::piecewise_construct 分次生成tuple的標(biāo)志常量
  // 在map::emplace()中使用forward_as_tuple時(shí)必須要加piecewise_construct,不加就報(bào)錯(cuò)
  // https://www.cnblogs.com/guxuanqing/p/11396511.html
  // 以1ms, 以及重力常數(shù)10, 作為參數(shù)構(gòu)造PoseExtrapolator
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

這里面有個(gè)重點(diǎn)變量:

std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;

看PoseExtrapolator這個(gè)類, 發(fā)現(xiàn)功能是使用IMU和/或里程計(jì)數(shù)據(jù)（如果有）來改善預(yù)測估計(jì)速度與運(yùn)動(dòng). 因?yàn)闄C(jī)器人運(yùn)動(dòng),角速度和線速度都不可能變化特別大, 所以可以用上一次的速度去預(yù)測下一次的位姿,然后用預(yù)測的位姿為初始值去進(jìn)行優(yōu)化, 這樣的好處是可以以較小的迭代次數(shù)獲得更好的結(jié)果,而且不容易陷入局部最小值.

數(shù)據(jù)采樣器

這一部分很簡單, 通過使用配置文件, 去給某條軌跡的各個(gè)傳感器得到的值進(jìn)行采樣,

/**
 * @brief 新生成一個(gè)傳感器數(shù)據(jù)采樣器
 * 
 * @param[in] trajectory_id 軌跡id
 * @param[in] options 參數(shù)配置
 */
void Node::AddSensorSamplers(const int trajectory_id,
                             const TrajectoryOptions& options) {
  CHECK(sensor_samplers_.count(trajectory_id) == 0);
  sensor_samplers_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          options.rangefinder_sampling_ratio, 
          options.odometry_sampling_ratio,
          options.fixed_frame_pose_sampling_ratio, 
          options.imu_sampling_ratio,
          options.landmarks_sampling_ratio));
}

看看sensor_samplers_, 定義如下

std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;

看看TrajectorySensorSamplers, 發(fā)現(xiàn)作用只有控制各個(gè)傳感器的采樣頻率.

訂閱話題與注冊回調(diào)函數(shù)

LaunchSubscribers(options, trajectory_id);

這個(gè)函數(shù)就挺有意思的, 有值得學(xué)的的編程技巧. 同樣, 傳入的參數(shù)只有配置文件和軌跡ID.

咱們進(jìn)入到node.cc里看這個(gè)函數(shù)本身, 發(fā)現(xiàn)它是首先通過配置options判斷了是否用了某個(gè)傳感器, 然后把SubscribeWithHandler壓入subscribers_里. 現(xiàn)在, 這里有兩個(gè)疑問, subscribers_是啥, SubscribeWithHandler又是啥. 咱們先看subscribers_

std::unordered_map<int, std::vector<Subscriber>> subscribers_;

subscribers_是一個(gè)無序表, 鍵是軌跡id, 值是一個(gè)數(shù)組, 里面放的都是Subscriber:

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

Subscriber是一個(gè)結(jié)構(gòu)體, 里面是ros的訂閱器和對應(yīng)的topic名字.

所以subscribers_表示某條軌跡的ros訂閱器以及訂閱topic的名字.

然后就是SubscribeWithHandler

/**
 * @brief 在node_handle中訂閱topic,并與傳入的回調(diào)函數(shù)進(jìn)行注冊
 * 
 * @tparam MessageType 模板參數(shù),消息的數(shù)據(jù)類型
 * @param[in] handler 函數(shù)指針, 接受傳入的函數(shù)的地址
 * @param[in] trajectory_id 軌跡id
 * @param[in] topic 訂閱的topic名字
 * @param[in] node_handle ros的node_handle
 * @param[in] node node類的指針
 * @return ::ros::Subscriber 訂閱者
 */
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    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構(gòu)造回調(diào)函數(shù),被subscribe注冊
      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表達(dá)式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

這個(gè)地方有點(diǎn)晦澀, 不過特別優(yōu)美, 也是值得學(xué)習(xí)的部分, 通過SubscribeWithHandler這個(gè)函數(shù), 實(shí)現(xiàn)了所有傳感器的訂閱,回調(diào)函數(shù)注冊,以及訂閱器的維護(hù),簡潔明了.

首先, 定義了一個(gè)模板, 用來實(shí)現(xiàn)同一個(gè)函數(shù)適應(yīng)不同的傳感器類型. 返回值是ros的Subscriber類. 對于第一個(gè)參數(shù), 這是一個(gè)函數(shù)指針,也就是函數(shù)的地址. 第二個(gè)和第三個(gè)就是軌跡id和message的topic, 第四個(gè)是ros的nodehandle, 掌控ros節(jié)點(diǎn)的開啟與關(guān)閉,讓我們能使用ros的Subscribe, 不重要. 最后一個(gè)參數(shù)就是當(dāng)前node類本身,讓我們可以在后續(xù)使用node類相關(guān)的方法和變量.

咱們以LaserScan為例看看

    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});

整體看來,就是通過SubscribeWithHandler這個(gè)模板函數(shù),把LaserScan類型的message, 回調(diào)函數(shù), 軌跡id, 訂閱話題名稱等打包成ros的subscriber, 連同訂閱話題名稱一起壓入某條軌跡的訂閱維護(hù)器.

看看這個(gè)回調(diào)函數(shù)咋傳的: 參數(shù)就是回調(diào)函數(shù)指針(地址), 而回調(diào)函數(shù)指針的定義在這里定義:

void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&)

可以看到,這個(gè)函數(shù)是無返回值,函數(shù)指針名叫handler的, 傳入?yún)?shù)類型為int, string,和當(dāng)前模板的傳感器類常量指針.

在看看SubscribeWithHandler. 第一個(gè)參數(shù)就是接收到LaserScan這個(gè)類型的message之后執(zhí)行的回調(diào)函數(shù)的地址,讓ros的Subscribe有回調(diào)函數(shù)調(diào)用, 后面的參數(shù)在上面已經(jīng)提到, 就不多說了. 把SubscribeWithHandler的定義和使用對照著看, 發(fā)現(xiàn)返回值是一個(gè)ros的subscribe, 寫法上和我們自己寫最基礎(chǔ)的ros訂閱是一樣的, topic+數(shù)字+回調(diào)函數(shù). 回調(diào)函數(shù)用的是boost::function, 作用和std::function差不多, 用來構(gòu)造函數(shù). 函數(shù)本身又是一個(gè)lambda表達(dá)式:

[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg)
{(node->*handler)(trajectory_id, topic, msg);}

這個(gè)lambda捕獲了外部的node類本身,也就是this, 軌跡id, topic名字, 傳入了當(dāng)前message的類型, 執(zhí)行了該回調(diào)函數(shù)(HandleLaserScanMessage), 注意啊,現(xiàn)在node->*handler就是HandleLaserScanMessage了, 因?yàn)樵趥鬟M(jìn)SubscribeWithHandler的時(shí)候就確定了這個(gè)函數(shù)指針是啥. 所以再來看看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);
}

和定義的函數(shù)指針一樣傳入?yún)?shù)類型為int, string,和當(dāng)前模板的傳感器類常量指針. 然后把這些數(shù)據(jù)MapBuilder的HandleLaserScanMessage進(jìn)行處理. MapBuilder這一部分內(nèi)容較多,還是要單獨(dú)講.

到此這篇關(guān)于C++Node類Cartographer開始軌跡的處理深度詳解的文章就介紹到這了,更多相關(guān)C++ Cartographer開始軌跡內(nèi)容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！

您可能感興趣的文章: