cartographer发布当前机器人在map中的位置

1.打开publish_tracked_pose=true, --发布小车位置

订阅 /tracked_pose就是小车位置信息

 

 

2.查看代码 /cartographer_ros/cartographer_ros/cartographer_ros/node.cc

//初始化位置
if (node_options_.publish_tracked_pose) {
    tracked_pose_publisher_ =
        node_handle_.advertise<::geometry_msgs::PoseStamped>(
            kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);// 发布tracked_pose, 默认不发布
  }

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

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

    geometry_msgs::TransformStamped stamped_transform;
    // If we do not publish a new point cloud, we still allow time of the
    // published poses to advance. If we already know a newer pose, we use its
    // time instead. Since tf knows how to interpolate, providing newer
    // information is better.

    // 使用较新的时间戳
    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);

    // Suppress publishing if we already published a transform at this time.
    // Due to 2020-07 changes to geometry2, tf buffer will issue warnings for
    // repeated transforms with the same timestamp.
    if (last_published_tf_stamps_.count(entry.first) &&
        last_published_tf_stamps_[entry.first] ==
            stamped_transform.header.stamp)
      continue;

    // 保存当前的时间戳, 以防止对同一时间戳进行重复更新
    last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp;

    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;

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