C++Node类Cartographer开始轨迹的处理深度详解

目录

• Node.h头文件
• Node类的构造函数
• 开始一条轨迹
• 添加一条轨迹
• 添加传感器维护功能:
• 添加一个轨迹
• 新增位姿估计器
• 数据采样器
• 订阅话题与注册回调函数

上一节我们看了一下node_main.cc的内容,发现其中最重要的部分就是Node类,这个类吃下了map_builder类,且里面实现了传感器数据的处理与分发(sensor_bridge),还有整个Cartographer算法的调用(map_builder与map_builder_bridge). 这一节我们重点看一下Node类的初始化和开始轨迹部分到底做了些什么.

Node.h头文件

在Node.h中,包含了以下几个部分(程序太大,就不贴了):

构造,析构,拷贝构造,赋值函数的构建

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: 禁止编译器自动生成默认函数; =default: 要求编译器生成一个默认函数
  // 禁止编译器自动生成 默认拷贝构造函数(复制构造函数)
  Node(const Node&) = delete;
  // 禁止编译器自动生成 默认赋值函数
  Node& operator=(const Node&) = delete;

轨迹有关部分

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

传感器数据部分

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

其他部分.

比如传感器采样设置,位姿推测器等部分.

Node类的构造函数

/**
 * @brief
 * 声明ROS的一些topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定
 *
 * @param[in] node_options 配置文件的内容
 * @param[in] map_builder SLAM算法的具体实现
 * @param[in] tf_buffer tf
 * @param[in] collect_metrics 是否启用metrics,默认不启用
 */
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_上锁, 防止在初始化时数据被更改
  absl::MutexLock lock(&mutex_);
  // 默认不启用
  if (collect_metrics) {
    metrics_registry_ = absl::make_unique<metrics::jsFamilyFactory>();
    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
  }
  // Step: 1 声明需要发布的topic
  // 发布SubmapList
  submap_list_publisher_ =
      node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
          kSubmapListTopic, kLatestOnlyPublisherQueueSize);
  // 发布轨迹
  trajectory_node_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
  // 发布landmark_pose
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
  // 发布约束
  constraint_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kConstraintListTopic, kLatestOnlyPublisherQueueSize);
  // 发布tracked_pose, 默认不发布
  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 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
...
  // Step: 3 处理之后的点云的发布器
...
  // Step: 4 进行定时器与函数的绑定, 定时发布数据
...
  // 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构造函数最重要的两个传入变量是node_options和map_builder. 这两个在上一节中已经详细说过了,map_builder是Cartographer算法部分,包含了前端和后端.

构造函数通过使用初始化列表去初始化一些私有变量(node_options_, map_builder_bridge_), 然后使用MutexLock上锁. 初始化列表和智能锁比较基础就不详细介绍了.

总之,这个构造函数使用node_options初始化了map_builder_bridge, 而map_builder_bridge又调用Cartographer算法部分的map_builder(前,后端), 同时还确定了要发布的topic,和可视化所需的topic.

开始一条轨迹

现在介绍一下Node::AddTrajectory。这块函数是这一节的重中之重了,是node.cc中的核心部分. 它维护了传感器列表, 添加了一条轨迹,新增了一个位姿估计器,传感器数据采样器,还有订阅所需的topic和注册对应的回调函数. 为了确保topic没有重复,他还保存了注册topic的键值对,以供查询是否重复.

添加一条轨迹

添加轨迹的函数是AddTrajectory

/**
 * @brief 添加一个新的轨迹
 *
 * @param[in] options 轨迹的参数配置
 * @return int 新生成的轨迹的id
 */
int Node::AddTrajectory(const TrajectoryOptions& options);

添加传感器维护功能:

调用了Node的函数ComputeExpectedSensorIds, 作用是根据配置文件,去返回一个传感器列表:

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的名字
      ......
    };

它规定了一个传感器的类型与一个对应的topic的名字. 传感器的类型是一个限域枚举(枚举类). 总之作用就是联系topic与topic对应的传感器类型, 以便后续维护.

回到之前的代码,不难看出, 一个轨迹的传感器的列表有一下命名规则:

如果只有一个传感器, 那订阅的topic就是topic

如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推(多个超声雷达)

3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个

里程计可有可无, topic的个数只能有一个

gps可有可无, topic的个数只能有一个

Landmark可有可无, topic的个数只能有一个

添加一个轨迹

接下来就是最重要的函数, AddTrajectory, Cartographer的核心, 传感器数据和Cartographer算法库连接处的大门, 调用了ros部分的map_builder_bridge和算法部分的map_builder产生联系. 联系的实现方法相当复杂, 将在另一节详细说

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

这一行调用了Map_builder_bridge_的AddTrajectory添加一条轨迹. 传入的参数有一个std::set<...Sensor_id>类型的变量,std::set是一个容器,可以简单理解为键值对,而键就是值,值就是键.(比较基础不细说啦).另一个就是从node_main.cc就跟着我们的TrajectoryOptions. 也就是配置文件读取的内容. 返回值很简单,就是新建轨迹的编号. Cartographer允许有多个轨迹同时维护,而且后面我们会发现, Cartographer定位其实就是把建好的地图和定位作为两个不同的轨迹实现的. 这个函数是整个Cartographer的功能实现, 方法很复杂, 将会在以后MapBuilder部分详细说.

新增位姿估计器

这个函数功能是通过IMU和里程计(轮编码器)去预估下一次可能的位姿,给定位一个初始值

/**
 * @brief 新增一个位姿估计器
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[i编程客栈n] options 参数配置
 */
void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
  // 新生成的轨迹的id 不应该在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()
         http://www.devze.com ? 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() 用于通过在容器中插入新元素来扩展map容器
  // 元素是直接构建的（既不复制也不移动）.仅当键不存在时才进行插入
  // c++11: std::forward_as_tuple tuple的完美转发
  // 该 tuple 在以右值为参数时拥有右值引用数据成员, 否则拥有左值引用数据成员
  // c++11: std::piecewise_construct 分次生成tuple的标志常量
  // 在map::emplace()中使用forward_as_tuple时必须要加piecewise_construct,不加就报错
  // https://www.cnblogs.com/guxuanqing/p/11396511.html
  // 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

这里面有个重点变量:

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

看PoseExtrapolator这个类, 发现功能是使用IMU和/或里程计数据（如果有）来改善预测估计速度与运动. 因为机器人运动,角速度和线速度都不可能变化特别大, 所以可以用上一次的速度去预测下一次的位姿,然后用预测的位姿为初始值去进行优化, 这样的好处是可以以较小的迭代次数获得更好的结果,而且不容易陷入局部最小值.

数据采样器

这一部分很简单, 通过使用配置文件, 去给某条轨迹的各个传感器得到的值进行采样,

/**
 * @brief 新生成一个传感器数据采样器
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[in] options 参数配置
 */
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, 发现作用只有控制各个传感器的采样频率.

订阅话题与注册回调函数

LaunchSubscribers(options, trajectory_id);

这个函数就挺有意思的, 有值得学的的编程技巧. 同样, 传入的参数只有配置文件和轨迹ID.

咱们进入到node.cc里看这个函数本身, 发现它是首先通过配置options判断了是否用了某个传感器, 然后把SubscribeWithHandler压入subscribers_里. 现在, 这里有两个疑问, subscribers_是啥, SubscribeWithHandler又是啥. 咱们先看subscribers_

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

subscribers_是一个无序表, 键是轨迹id, 值是一个数组, 里面放的都是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是一个结构体, 里面是ros的订阅器和对应的topic名字.

所以subscribers_表示某条轨迹的ros订阅器以及订阅topic的名字.

然后就是SubscribeWithHandler

/**
 * @brief 在node_handle中订阅topic,并与传入的回调函数进行注册
 * 
 * @tparam MessageType 模板参数,消息的数据类型
 * @param[in] handler 函数指针, 接受传入的函数的地址
 * @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构造回调函数,被subscribe注册
      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表达式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr&www.devze.com; msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

这个地方有点晦涩, 不过特别优美, 也是值得学习的部分, 通过SubscribeWithHandler这个函数, 实现了所有传感器的订阅,回调函数注册,以及订阅器的维护,简洁明了.

首先, 定义了一个模板, 用来实现同一个函数适应不同的传感器类型. 返回值是ros的Subscriber类. 对于第一个参数, 这是一个函数指针,也就是函数的地址. 第二个和第三个就是轨迹id和message的topic, 第四个是ros的nodehandle, 掌控ros节点的开启与关闭,让我们能使用ros的Subscribe, 不重要. 最后一个参数就是当前node类本身,让我们可以在后续使用node类相关的方法和变量.

咱们以LaserScan为例看看

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

整体看来,就是通过SubscribeWithHandler这个模板函数,把LaserScan类型的message, 回调函数, 编程客栈轨迹id, 订阅话题名称等打包成ros的subscriber, 连同订阅话题名称一起压入某条轨迹的订阅维护器.

看看这个回调函数咋传的: 参数就是回调函数指针(地址), 而回调函数指针的定义在这里定义:

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

可以看到,这个函数是无返回值,函数指针名叫handler的, 传入参数类型为int, string,和当前模板的传感器类常量指针.

在看看SubscribeWithHandler. 第一个参数就是接收到LaserScan这个类型的message之后执行的回调函数的地址,让ros的Subscribe有回调函数调用, 后面的参数在上面已经提到, 就不多说了. 把SubscribeWithHandler的定义和使用对照着看, 发现返回值是一个ros的subscribe, 写法上和我们自己写最基础的ros订阅是一样的, topic+数字+回调函数. 回调函数用的是boost::function, 作用和std::function差不多, 用来构造函数. 函数本身又是一个lambda表达式:

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

这个lambda捕获了外部的node类本身,也就是this, 轨迹id, topic名字, 传入了当前message的类型, 执行了该回调函数(HandleLaserScanMessage), 注意啊,现在node->*handler就是HandleLaserScanMessage了, 因为在传进SubscribeWithHandler的时候就确定了这个函数指针是啥. 所以再来看看HandleLaserScanMessage

// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 根据配置,是否将传感器数据跳过
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

和定义的函数指针一样传入参数类型为int, string,和当前模板的传感器类常量指针. 然后把这些数据MapBuilder的HandleLaserScanMessage进行处理. MapBuilder这一部分内容较多,还是要单独讲.

到此这篇关于C++Node类Cartographer开始轨迹的处理深度详解的文章就介绍到这了,更多相关C++ Cartographer开始轨迹内容请搜索我们以前的文章或继续浏览下面的相关文章希望大开发者_JS开发家以后多多支持我们！