定时发布数据-Node类-cartographer
Kong Liangqian Lv6
 2021-09-05 15:59:22  

定时器定义

调用Node的构造函数的时候，顺便定义了发布者，可查看node.cc文件Node::Node。submaplist_publisher即为一个::ros::Publisher

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

  // 发布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);
  ...
  // Step: 4 进行定时器与函数的绑定, 定时发布数据
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),  // 0.3s
      &Node::PublishSubmapList, this));
}

之后对定时器和要发布函数进行绑定。下面这句的意思为每隔0.3秒即可调用函数Node::PublishSubmapList一次

1
2
3
node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),  // 0.3s
      &Node::PublishSubmapList, this);

定时器函数绑定Node::PublishSubmapList

通过map_builder_bridge来获取所有SubmapList的消息，然后进行消息的发布

1
2
3
4
5
6
7
8
9
10
/**
 * @brief 每0.3s发布一次submap list,
 * 这里的submap只有节点的id与当前submap的节点数, 并没有地图数据
 *
 * @param[in] unused_timer_event
 */
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
  absl::MutexLock lock(&mutex_);
  submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
}

他发布的消息是类型可以在声明的时候看见为::cartographer_ros_msgs::SubmapList，可以在cartographer_ros_msgs里面可以查看

1
2
std_msgs/Header header
cartographer_ros_msgs/SubmapEntry[] submap

再去查看SubmapEntry

1
2
3
4
5
int32 trajectory_id
int32 submap_index
int32 submap_version
geometry_msgs/Pose pose
bool is_frozen

这样就可以完全清除发布消息的内容

轨迹路径定时发布

这里首先判断是否有订阅者对其进行订阅，如果没有订阅者，那么就不进行发布了，可以节约资源。

如果有订阅者额，那么就和submaplist一样通过map_builder_bridge来获取所有GetTrajectoryNodeList的消息然后进行发布。

发布的消息类型格式可以通过声明此发布者的时候看见为::visualization_msgs::MarkerArray

1
2
3
4
5
6
7
8
9
10
// 每30e-3s发布一次轨迹路径点数据
void Node::PublishTrajectoryNodeList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  // 只有存在订阅者的时候才发布轨迹
  if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    trajectory_node_list_publisher_.publish(
        map_builder_bridge_.GetTrajectoryNodeList());
  }
}

约束发布

这里和轨迹发布的逻辑完全一样，

1
2
3
4
5
6
7
// 每0.5s发布一次约束数据
void Node::PublishConstraintList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  if (constraint_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
  }

landmarkPose发布

这里和轨迹发布的逻辑完全一样，

1
2
3
4
5
6
7
8
9
// 每30e-3s发布一次landmark pose 数据
void Node::PublishLandmarkPosesList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    landmark_poses_list_publisher_.publish(
        map_builder_bridge_.GetLandmarkPosesList());
  }
}

局部轨迹数据发布

此函数的目的就是发布tf和跟踪位姿。tf为根据配置选择的

  • map->odom->published_frame.
  • map ->published_frame
1
void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event)

等以后讲解完前后端之后再来讲解

 Comments