slam建图与定位_cartographer代码阅读(2)_数据走向和node类

一.C++相关

c++11: =delete: 禁止编译器自动生成默认函数; =default: 要求编译器生成一个默认函数

  // 禁止编译器自动生成 默认拷贝构造函数(复制构造函数)
  Node(const Node&) = delete;
  // 禁止编译器自动生成 默认赋值函数
  Node& operator=(const Node&) = delete;

GUARDED_BY 线程安全属性

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

3.数据结构：std::unordered_map 是采用哈希数据结构实现的， std::map是通过红黑树实现的

		  // c++11: std::unordered_map 是采用哈希数据结构实现的, 内部数据保存是无序的
		  // 相对于std::map, unordered_map的查找和插入的效率高, 时间复杂度为常数级别O(1),而额外空间复杂度则要高出许多
		  // 对于需要高效率查询的情况, 使用unordered_map容器, 但是unordered_map对于迭代器遍历效率并不高
	
	  // These are keyed with 'trajectory_id'.
	  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
	  std::map<int, ::ros::Time> last_published_tf_stamps_;
	  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
	  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
	  std::unordered_set<std::string> subscribed_topics_;
	  std::unordered_set<int> trajectories_scheduled_for_finish_;

二.Node类初始化

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
 Node node(node_options, std::move(map_builder), &tf_buffer,
        FLAGS_collect_metrics);

三.Node类作用,区分为3个模块
1.话题、地图文件、轨迹，保存、检查、使用；ros服务的处理；定时发布数据2.开始轨迹、结束轨迹；处理传感器数据3.构造函数，有4个模块(声明ROS的一些topic的发布器;服务的发布器;处理之后的点云的发布器;以及将时间驱动的函数与定时器进行绑定)

/**
 * @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());
  }

// Step: 1 声明需要发布的topic 如果想更改就使用remap或者更改代码
//话题名在/cartographer_ros/node_constants.h里定义
constexpr char kSubmapListTopic[] = “submap_list”;

  // 发布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);
  //pub的定位姿态
    _pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("pose_nav", 10);//100hz   

  // 发布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容器中，地图信息的处理

	  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 处理之后的点云的发布器

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

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

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

slam建图与定位_cartographer代码阅读(2)_数据走向和node类

猜你喜欢