cartographer代码学习笔记-node.LaunchSubscribers

Running小学生

已于 2023-12-06 14:24:41 修改

阅读量65

点赞数

分类专栏： cartographer 文章标签：学习笔记

于 2023-11-30 19:22:12 首次发布

本文链接：https://blog.csdn.net/weixin_28683491/article/details/134719477

版权

cartographer 专栏收录该内容

19 篇文章 1 订阅

订阅专栏

文章讲述了CartographerROS节点如何订阅和处理来自激光雷达、IMU、odom和landmarks的数据，使用`SubscribeWithHandler`创建订阅者，并调用相应的回调函数进行数据处理。

摘要由CSDN通过智能技术生成

该函数主要功能是定语传感器数据并调用回调函数处理

代码解析

解析激光数据

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  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});
  }
  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});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

激光数据

对于激光传感器可能存在多个传感器话题，通过for循环设置其话题名。以单激光为例，多线激光和点云同理。
首先，通过ComputeRepeatedTopicNames对多个激光话题设置

std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
                                                   const int num_topics) {
  CHECK_GE(num_topics, 0);//没有，return
  if (num_topics == 1) {//只有一个返回话题本身
    return {topic};
  }
  //多个传感器设置为topic_1,topic_2....
  std::vector<std::string> topics;
  topics.reserve(num_topics);
  for (int i = 0; i < num_topics; ++i) {
    topics.emplace_back(topic + "_" + std::to_string(i + 1));
  }
  return topics;//返回话题vector
}

上述代码主要判断参数中设定的激光数量是否>=1，否则就退出。
该参数是在lua文件中设置。一般只有1个

  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_point_clouds = 0,

其次，通过SubscribeWithHandler将订阅的话题，回调函数等处理返回ros::Subscriber类型的结果添加到subscribers_中

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

Subscriber结构体定义

  struct Subscriber {
    ::ros::Subscriber subscriber; //ros定语器

    // ::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; //订阅话题
  };

SubscribeWithHandler定义
这是一个模板函数，主要实现是ros的订阅机制

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,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

imu处理

需要根据2d还是3d来选择，3d必须使用imu，2d可以不使用imu，如果使用imu需要设置use_imu_data = true。

  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, kImuTopic,
                                                &node_handle_, this),
         kImuTopic});
  }

里程计 gps landmarks

三则处理结构一样

  if (options.use_odometry) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, kOdometryTopic,
                                                  &node_handle_, this),
         kOdometryTopic});
  }
  if (options.use_nav_sat) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
             &node_handle_, this),
         kNavSatFixTopic});
  }
  if (options.use_landmarks) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
             &node_handle_, this),
         kLandmarkTopic});
  }