cartographer-Node类
Kong Liangqian Lv6
 2021-08-28 15:59:22  

c++11 新特性

c++11 类默认函数的控制：”=default” 和 “=delete”函数

以下内容参考https://www.cnblogs.com/lsgxeva/p/7787438.html

C++ 的类有四类特殊成员函数，它们分别是：默认构造函数、析构函数、拷贝构造函数以及拷贝赋值运算符。
这些类的特殊成员函数负责创建、初始化、销毁，或者拷贝类的对象。
如果程序员没有显式地为一个类定义某个特殊成员函数，而又需要用到该特殊成员函数时，则编译器会隐式的为这个类生成一个默认的特殊成员函数。

简单的说，使用=default，会使用相对应的默认函数，使用=delete，则禁止生成相对应的默认函数，如果想要实现，则自己必须定义

1
2
3
4
5
6
7
8
9
10
11
12
class X
{ 
public: 
    X() = default; //该函数比用户自己定义的默认构造函数获得更高的代码效率
    X(int i)
    { 
        a = i; 
    }
    ~X() = default;
    private: 
    int a = 0; 
};

调用了=default，则就不需要在实现一个X()，如果是X() = delete;，那么

1
X obj = X();

则会报错。

除了默认构造函数和析构函数，还有默认拷贝构造函数和默认拷贝赋值操作符也可以应用，都是有默认的函数实现的

1
2
3
4
5
6
7
8
9
10
11
class X2
{
public:
    X2() = default; //Inline defaulted 默认构造函数
    X2(const X&);
    X2& operator = (const X&);
    ~X2() = default;  //Inline defaulted 析构函数
};

X2::X2(const X&) = default;  //Out-of-line defaulted 拷贝构造函数
X2& X2::operator= (const X2&) = default; //Out-of-line defaulted  拷贝赋值操作符

为了能够让程序员显式的禁用某个函数，C++11 标准引入了一个新特性：”=delete”函数。程序员只需在函数声明后上“=delete;”，就可将该函数禁用。

1
2
3
4
5
6
class X5
{
public:
    void *operator new(size_t) = delete;
    void *operator new[](size_t) = delete;
};

则再调用此函数就会报错

1
2
X5 *pa = new X5;      // 错误，new 操作符被禁用
X5 *pb = new X5[10];  // 错误，new[] 操作符被禁用

C++：vector中的resize()函数 VS reserve()函数

C++: push_back 和 emplace_back

Node 类

在node_main.cc中，Run函数调用了Node了构造函数，此Node，是用于将ROS的topic传入SLAM，我们首先简答看一下Node的内容

1
2
Node node(node_options, std::move(map_builder), &tf_buffer,
          FLAGS_collect_metrics);

打开Node类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
class Node {
 public:
  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;
  ...
}

这里所使用的delete，是指不用编译器自动生成的默认函数。

构造函数

此构造函数的主要工作有

声明ROS的一些topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
/**
 * @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::FamilyFactory>();
    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
  }

上面一部分内容，对node_option参数配置进行了赋值，对MapBuilderBridge的对象进行初始化，上锁！，注意下面的定义，GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护。对数据的读操作需要共享访问, 而写操作则需要互斥访问.

GUARDED_BY

1
2
3
4
5
// c++11: GUARDED_BY 是在Clang Thread Safety Analysis（线程安全分析）中定义的属性
// GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护.
// 对数据的读操作需要共享访问, 而写操作则需要互斥访问.
// 官方介绍文档: https://clang.llvm.org/docs/ThreadSafetyAnalysis.html
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

Step: 1 声明需要发布的topic

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// 发布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);
}

Step: 2 声明发布对应名字的ROS服务

1
2
3
4
5
6
7
8
9
10
11
12
13
14
service_servers_.push_back(node_handle_.advertiseService(
    kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
    kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
    kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
    kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
    kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
    kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
    kReadMetricsServiceName, &Node::HandleReadMetrics, this));

Step: 3 处理之后的点云的发布器

1
2
3
scan_matched_point_cloud_publisher_ =
    node_handle_.advertise<sensor_msgs::PointCloud2>(
        kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);

Step: 4 进行定时器与函数的绑定, 定时发布数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
wall_timers_.push_back(node_handle_.createWallTimer(
    ::ros::WallDuration(node_options_.submap_publish_period_sec),  // 0.3s
    &Node::PublishSubmapList, this));
if (node_options_.pose_publish_period_sec > 0) {
  publish_local_trajectory_data_timer_ = node_handle_.createTimer(
      ::ros::Duration(node_options_.pose_publish_period_sec),  // 5e-3s
      &Node::PublishLocalTrajectoryData, this);
}
wall_timers_.push_back(node_handle_.createWallTimer(
    ::ros::WallDuration(
        node_options_.trajectory_publish_period_sec),  // 30e-3s
    &Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
    ::ros::WallDuration(
        node_options_.trajectory_publish_period_sec),  // 30e-3s
    &Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
    ::ros::WallDuration(kConstraintPublishPeriodSec),  // 0.5s
    &Node::PublishConstraintList, this));

注意，wall_timer直译是墙上的时间，在墙上挂着的时钟是不会改变的，因此他不是模拟的时间。

这里的函数是以时间为驱动的，譬如SubmapList是每隔0.3s一次进行发布

开始轨迹

1
2
3
4
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
  node.StartTrajectoryWithDefaultTopics(trajectory_options);
}

传入的是trajectory_options，他包含着和轨迹相关的参数，开始绘制一条轨迹

StartTrajectoryWithDefaultTopics

1
2
3
4
5
6
7
8
// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

开始轨迹之前，一定要确定给定的是一条2d轨迹还是一条3d轨迹

ValidateTrajectoryOptions

1
2
3
4
5
6
7
8
9
10
11
12
// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_2d_options();
  }
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_3d_options();
  }
  return false;
}

如果配置了MAP_BUILDER.use_trajectory_builder_2d = true 或者 MAP_BUILDER.use_trajectory_builder_3d = true，这里就会返回true，否则跳过两个if，直接返回false，直接结束程序

AddTrajectory

上述检查通过，即参数配置没有问题了，那么开始添加一条轨迹

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
int Node::AddTrajectory(const TrajectoryOptions& options) {

  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

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

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);

  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);

  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
}

首先需要确定传进来的参数options中含有哪些传感器，以及对应的topic名字

SensorId

通过查看SensorId可知

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
struct SensorId {
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    SensorType type;
    std::string id;

    bool operator==(const SensorId& other) const {
      return std::forward_as_tuple(type, id) ==
             std::forward_as_tuple(other.type, other.id);
    }

    bool operator<(const SensorId& other) const {
      return std::forward_as_tuple(type, id) <
             std::forward_as_tuple(other.type, other.id);
    }
  };

SensorId里面包含这两个变量名，expected_topics一个是 SensorType type，另外一个是std::string id，就是对应sensor的默认topic名字。其中type的类型在SensorType中定义，注意最后一个是局部SLAM结果即前端的结果。

这里也定义了SensorId的比较运算符，通过std::forward_as_tuple来对多个数字进行比较。

ComputeExpectedSensorIds
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
/**
 * @brief 根据配置文件, 确定所有需要的topic的名字的集合
 *
 * @param[in] options TrajectoryOptions的配置文件
 * @return std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
 */
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
  /*
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    struct SensorId {
      SensorType type;  // 传感器的种类
      std::string id;   // topic的名字
    };
  */
 
  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
  using SensorType = SensorId::SensorType;
  std::set<SensorId> expected_topics;
  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.

  // 如果只有一个传感器, 那订阅的topic就是topic
  // 如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  // 3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个
  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())) {
    expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
  }
  // Odometry is optional.
  // 里程计可有可无, topic的个数只能有一个
  if (options.use_odometry) {
    expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
  }
  // NavSatFix is optional.
  // gps可有可无, topic的个数只能有一个
  if (options.use_nav_sat) {
    expected_topics.insert(
        SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
  }
  // Landmark is optional.
  // Landmark可有可无, topic的个数只能有一个
  if (options.use_landmarks) {
    expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
  }
  // 返回传感器的topic名字
  return expected_topics;
}

其返回结果是类型为SensorId的一个集合，每一个SensorId应该包含传感器的种类和默认topic的名字。

1
std::set<SensorId> expected_topics;

对每一个传感器进行判断，比如num_laser_scans=2，那订阅的激光雷达topic名字就是topic_1,topic_2, 依次类推。点云的

订阅数量也是一样。points2_1、points2_2等等。

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

mapbuilder_bridge变量在构造函数的时候已经初始化。

位姿估计器

此位姿估计器为IMU和里程计的一个融合，他只是去做前端匹配之前的一个先验

gravity_time_constant

1
2
3
4
5
6
7
// 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();

首先是对重力常数的一个配置，无论是3D的还是2D的都是10。相应的配置在`cartographer/configuration_files/trajectory_builder_2d.lua文件中，有

1
imu_gravity_time_constant = 10.,

以及相应的3D文件中，也有同样的配置。

extrapolators_

extrapolators_被定义为

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

下面是对extrapolators_进行插入操作，如果没有这个key，才进行插入操作。

c++11: map::emplace() 用于通过在容器中插入新元素来扩展map容器

1
2
3
4
5
6
7
// 以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));
传感器数据采样器

在Node::AddTrajectory中，创建了传感器数据采样器

1
2
// 新生成一个传感器数据采样器
AddSensorSamplers(trajectory_id, options);

这也是一个Node的函数，定义如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
 * @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));
}

sensorsamplers

在Node中的sensorsamplers定义如下

1
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;

他依然是由轨迹ID作为key，但是是由TrajectorySensorSamplers作为value进行保存。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// 控制各个传感器数据的采样频率
struct TrajectorySensorSamplers {
  TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
                           const double odometry_sampling_ratio,
                           const double fixed_frame_pose_sampling_ratio,
                           const double imu_sampling_ratio,
                           const double landmark_sampling_ratio)
      : rangefinder_sampler(rangefinder_sampling_ratio),
        odometry_sampler(odometry_sampling_ratio),
        fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
        imu_sampler(imu_sampling_ratio),
        landmark_sampler(landmark_sampling_ratio) {}

  ::cartographer::common::FixedRatioSampler rangefinder_sampler;
  ::cartographer::common::FixedRatioSampler odometry_sampler;
  ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
  ::cartographer::common::FixedRatioSampler imu_sampler;
  ::cartographer::common::FixedRatioSampler landmark_sampler;
};

TrajectorySensorSamplers保存了五种设备的采样器。在调用构造函数的时候进行初始化.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
class FixedRatioSampler {
 public:
  explicit FixedRatioSampler(double ratio);
  ~FixedRatioSampler();

  FixedRatioSampler(const FixedRatioSampler&) = delete;
  FixedRatioSampler& operator=(const FixedRatioSampler&) = delete;

  // Returns true if this pulse should result in an sample.
  bool Pulse();

  // Returns a debug string describing the current ratio of samples to pulses.
  std::string DebugString();

 private:
  // Sampling occurs if the proportion of samples to pulses drops below this
  // number.
  const double ratio_;

  int64 num_pulses_ = 0;
  int64 num_samples_ = 0;
};

Pulse()函数返回一个布尔值，如果是true代表此帧需要使用，如果是false则丢弃

1
2
3
4
5
6
7
8
9
10
// 在比例小于ratio_时返回true, ratio_设置为1时都返回true, 也就是说使用所有的数据
bool FixedRatioSampler::Pulse() {
  ++num_pulses_;
  if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
    ++num_samples_;
    return true;
  }
  // 返回false时代表数据可以不用,可以跳过计算
  return false;
}

这个东西十分好理解。这里控制两个变量，分母num_pulses_越来越大，

因此，总的值会越来越小，直到小于ratio，一旦小于了ratio，采样值，则分子又变大。

 Comments