(02)Cartographer源码无死角解析-(77) ROS数据发布→2D点云数据、tf、机器人tracking frame轨迹发布

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

上一篇博客中，讲解了3D点云地图、子图位姿、Landmark 数据消息的发布名，但是并没有对
Node::PublishLocalTrajectoryData()、Node::PublishTrajectoryNodeList()、Node::PublishConstraintList() 进行分析，不过需要注意一点。需要注意到node.cc 中构造函数的如下代码：

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

如果使用的是3D轨迹，才会调用 Node::PublishPointCloudMap 发布 3D点云数据的画图。对于2D点云数据的发布是通过以一定频率调用 Node::PublishLocalTrajectoryData() 这个函数。在讲解之前呢，回顾一个知识点，那就是添加一条轨迹的时，调用到 MapBuilderBridge::AddTrajectory() 函数时，存在如下代码：

  // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // lambda表达式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });

其往前端优化中注册了一个回调函数 OnLocalSlamResult()，每次前端扫描匹配进行节点位姿优化之后，都会把相关的结果与数据通过 OnLocalSlamResult() 在 MapBuilderBridge::local_slam_data_ 这个变量中，这里保存着每条轨迹轨迹最新扫描匹配的结果。

系统分发数据的时候，会调用到 src/cartographer/cartographer/sensor/internal/dispatchable.h 中的Dispatchable::AddToTrajectoryBuilder() 而执行到 GlobalTrajectoryBuilder::AddSensorData() 函数。该函数中会把点云数据添加到前端获得扫描匹配的结果，然后会执行如下代码：

      // 将匹配后的结果 当做节点 加入到位姿图中
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps);

这样就把节点数据添加到后端，也就是存储到 PoseGraph2D::data_.trajectory_nodes 中，且后续后端优化中会对其global 位姿进行调整。有了这些基础之后正式来分析 Node::PublishLocalTrajectoryData() 函数。

二、数据获取

Node::PublishLocalTrajectoryData() 函数第一步执行的代码如下所示：

/**
 * @brief 每5e-3s发布一次tf与tracked_pose
 *
 * @param[in] timer_event
 */
void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
  absl::MutexLock lock(&mutex_);
  for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
    // entry的数据类型为std::unordered_map<int,MapBuilderBridge::LocalTrajectoryData>
    // entry.first 就是轨迹的id, entry.second 就是 LocalTrajectoryData

    // 获取对应轨迹id的trajectory_data
    const auto& trajectory_data = entry.second;

    // 获取对应轨迹id的extrapolator
    auto& extrapolator = extrapolators_.at(entry.first);

其主要就是通过调用 MapBuilderBridge::GetLocalTrajectoryData() 函数获得所有轨迹最后一个前端扫描匹配结果数据，然后分别进行遍历，先获得遍历节点轨迹相关的数据 trajectory_data，以及其对应的位姿推断器 extrapolator。需要注意，该处的位姿推断器是在添加轨迹 Node::AddTrajectory() 函数中调用 AddExtrapolator() 创建的。其与前端的位姿推断其并非同一个。前端 local 中的推断器是根据里程计odomery，IMU等数据集进行位姿推断，结合点云匹配的结果对 extrapolator 进行优化，该处的 extrapolator 只利用到了点云匹配结果位姿，相对来说，应该会更加准一些，如果频率足够高。

三、点云数据发布

只有点云数据发生改变的时候才会进行发布，所以发布点云数据的频率不会非常高，且避免了重复计算。主题代码如下：

    // We only publish a point cloud if it has changed. It is not needed at high
    // frequency, and republishing it would be computationally wasteful.
    // 如果当前状态的时间与extrapolator的lastposetime不等
    if (trajectory_data.local_slam_data->time !=
        extrapolator.GetLastPoseTime()) {
      // 有订阅才发布scan_matched_point_cloud
      if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
        // TODO(gaschler): Consider using other message without time
        // information.
        carto::sensor::TimedPointCloud point_cloud;
        point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
                                .returns.size());

        // 获取local_slam_data的点云数据, 填入到point_cloud中
        for (const cartographer::sensor::RangefinderPoint point :
             trajectory_data.local_slam_data->range_data_in_local.returns) {
          // 这里的虽然使用的是带时间戳的点云结构, 但是数据点的时间全是0.f
          point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
              point, 0.f /* time */));
        }

        // 先将点云转换成ROS的格式,再发布scan_matched_point_cloud点云
        scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
            carto::common::ToUniversal(trajectory_data.local_slam_data->time),
            node_options_.map_frame,
            // 将雷达坐标系下的点云转换成地图坐标系下的点云
            carto::sensor::TransformTimedPointCloud(
                point_cloud, trajectory_data.local_to_map.cast<float>())));
      }  // end 发布scan_matched_point_cloud

      // 将当前的pose加入到extrapolator中, 更新extrapolator的时间与状态
      extrapolator.AddPose(trajectory_data.local_slam_data->time,
                           trajectory_data.local_slam_data->local_pose);
    } // end if

$(1):$ 首先判断一下上一次轨迹数据对应的时间 trajectory_data.local_slam_data->time 其位姿是否等于最后一次获取位姿的时间，如果不是会进一步判断是否存在订阅者。

$(2):$ 创建一个一个时间 carto::sensor::TimedPointCloud 类型实例 point_cloud，且预分配好空间，然后把前端保存的点云数据 trajectory_data.local_slam_data->range_data_in_local.returns 进行转换添加到 point_cloud。

$(3):$ 将雷达坐标系下的点云转换成地图坐标系下的点云然后进行发布，获取到的点云数据是相对loval系的位姿，通过左乘 global 系到 local 的坐标变换 trajectory_data.local_to_map 把点云数据变换到 global 系进行发布

$(4):$ 把将当前扫描匹配local系下的pose加入到extrapolator中, 更新extrapolator的时间与状态。主要是利用 trajectory_data.local_slam_data->local_pose 数据推断线速度与角速度。到这里为止，2D点云数据的发布就已经完成了，接着就是发布机器人的位姿。

四、机器人位姿发布

$(1):$ 调用，Node::PublishLocalTrajectoryData() 函数即使没有发布点云数据，也会发布 tracked_pose(机器人位姿)，该位姿的发布主要通过位姿推断器 extrapolator 推断机器人 pose。器首先通过如下代码：

    // 使用较新的时间戳
    const ::cartographer::common::Time now = std::max(
        FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
    stamped_transform.header.stamp =
        node_options_.use_pose_extrapolator
            ? ToRos(now)
            : ToRos(trajectory_data.local_slam_data->time);
  
    // 保存当前的时间戳, 以防止对同一时间戳进行重复更新
    last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp;          

获得最 extrapolator 插值时间与目前 ros::Time::now() 比较之后的最新时间。last_published_tf_stamps_ 用于记录上一位姿发布的时间，也就是说判断一下该时刻的位姿是否发布过了，如果已经发布过则 continue 遍历下一条轨迹。总的来说，也是避免重复计算。

$(2):$ 获取当前机器人(tracking)在local系的坐标坐标。需要注意，如果设置了在 .lua 文件中配置了 published_frame = “base_link” 以及 publish_frame_projected_to_2d = true，那么发布机器人的轨迹就是以 base_link 节点为基准，而非 tracking_frame。另外，还会利用 local系到global系的坐标变换 trajectory_data.local_to_map，把机器人位姿变换到 global 系。代码注释如下：

    const Rigid3d tracking_to_local_3d =
        node_options_.use_pose_extrapolator
            ? extrapolator.ExtrapolatePose(now)
            : trajectory_data.local_slam_data->local_pose;
    
    // 获取当前位姿在local坐标系下的坐标
    const Rigid3d tracking_to_local = [&] {
      // 是否将变换投影到平面上
      if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
        return carto::transform::Embed3D(
            carto::transform::Project2D(tracking_to_local_3d));
      }
      return tracking_to_local_3d;
    }();

    // 求得当前位姿在map下的坐标
    const Rigid3d tracking_to_map =
        trajectory_data.local_to_map * tracking_to_local;

五、tf变换发布

先来看一下整体注释，在进行细节分析：

    // 根据lua配置文件发布tf
    if (trajectory_data.published_to_tracking != nullptr) {
      if (node_options_.publish_to_tf) {
        // 如果需要cartographer提供odom坐标系
        // 则发布 map_frame -> odom -> published_frame 的tf
        if (trajectory_data.trajectory_options.provide_odom_frame) {
          std::vector<geometry_msgs::TransformStamped> stamped_transforms;
          
          // map_frame -> odom
          stamped_transform.header.frame_id = node_options_.map_frame;
          stamped_transform.child_frame_id =
              trajectory_data.trajectory_options.odom_frame;
          // 将local坐标系作为odom坐标系
          stamped_transform.transform =
              ToGeometryMsgTransform(trajectory_data.local_to_map);
          stamped_transforms.push_back(stamped_transform);

          // odom -> published_frame
          stamped_transform.header.frame_id =
              trajectory_data.trajectory_options.odom_frame;
          stamped_transform.child_frame_id =
              trajectory_data.trajectory_options.published_frame;
          // published_to_tracking 是局部坐标系下的位姿
          stamped_transform.transform = ToGeometryMsgTransform(
              tracking_to_local * (*trajectory_data.published_to_tracking));
          stamped_transforms.push_back(stamped_transform);
          
          // 发布 map_frame -> odom -> published_frame 的tf
          tf_broadcaster_.sendTransform(stamped_transforms);
        } 
        // cartographer不需要提供odom坐标系,则发布 map_frame -> published_frame 的tf
        else {
          stamped_transform.header.frame_id = node_options_.map_frame;
          stamped_transform.child_frame_id =
              trajectory_data.trajectory_options.published_frame;
          stamped_transform.transform = ToGeometryMsgTransform(
              tracking_to_map * (*trajectory_data.published_to_tracking));
          // 发布 map_frame -> published_frame 的tf
          tf_broadcaster_.sendTransform(stamped_transform);
        }
      }
      

$(1):$ 如果 trajectory_data 有保存 published frame 到 tracking frame 之间的变换关系，则再一步判断是否有配置 publish_to_tf 参数，如果配置，则表明发布 tf 数据。

$(2):$ 若 trajectory_data.trajectory_options.provide_odom_frame 设置为 true，则表示有提供 odom_frame。则创建一个存储 geometry_msgs::TransformStamped 对象实例的容器 stamped_transforms。

$(3):$ 构建第一个 TransformStamped 对象，其表示 local 到 map(global) 系的坐标变换，注意这里的 stamped_transform.child_frame_id = trajectory_data.trajectory_options.odom_frame，也就是说认为 odom 的坐标系为 local 系。接着构建 odom(local) 到 published_frame 系到published_frame 的坐标变换。最后在进行发布。

总的来说，就是构建 map_frame(global) 到 odom(local) 再到 published_frame(机器人) 的坐标变换，理想情况下是 map_frame(global) 到 odom(local) 为单位变换，即两坐标系重合。

$(4):$ 如果没有配置 provide_odom_frame 参数，则只发布 map_frame 到 published_frame 的tf。

六、发布基于 tracking frame 的轨迹

      // publish_tracked_pose 默认为false, 默认不发布
      // 如果设置为true, 就发布一个在tracking_frame处的pose
      if (node_options_.publish_tracked_pose) {
        ::geometry_msgs::PoseStamped pose_msg;
        pose_msg.header.frame_id = node_options_.map_frame;
        pose_msg.header.stamp = stamped_transform.header.stamp;
        pose_msg.pose = ToGeometryMsgPose(tracking_to_map);
        tracked_pose_publisher_.publish(pose_msg);
      }

根据参数 node_options_.publish_tracked_pose 的配置决定是否发布。

七、结语

基于前面的分析，可以知道 Node::PublishLocalTrajectoryData() 主要内容为发布 D点云、tf、机器人tracking frame轨迹。当然，具体发布内容跟配置文件中的参数相关。下一篇博客就是对 Node::PublishConstraintList() 这个函数的细节分析了。