Autoware 定位之GEO坐标转换与带有IMU的里程计（五）

0. 简介

我们在上一篇中给我们的定位开了一个头，下面我们继续按照按照《Autoware 技术代码解读（三）》梳理的顺序，我们一个个慢慢来看与学习。这一讲我们来看GEO坐标转换与带有IMU的里程计。

1. GEO坐标转换

这个功能就只有一个geo_pose_projector.cpp文件，里面的内容也不是很多。该代码是一个用于将地理位置姿态信息转换为相对于某个参考框架的姿态信息的节点。它订阅了地图投影信息和地理位置姿态信息的话题，并根据接收到的信息进行姿态信息的转换和发布。在接收到地理位置姿态信息后，它提取出GPS坐标并将其投影到参考框架上，然后根据高程差异进行修正，并将修正后的姿态信息发布出去。如果设置了发布tf的标志，还会根据姿态信息发布tf变换信息。

 2./// @brief 地理位置姿态信息转换为相对于某个参考框架的姿态信息
GeoPoseProjector::GeoPoseProjector()
    : Node("geo_pose_projector"), publish_tf_(declare_parameter<bool>("publish_tf")) {
    // Subscribe to map_projector_info topic
    const auto adaptor = component_interface_utils::NodeAdaptor(this);
    adaptor.init_sub(
        sub_map_projector_info_,
        [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; });

    // Subscribe to geo_pose topic
    geo_pose_sub_ = create_subscription<GeoPoseWithCovariance>(
        "input_geo_pose",
        10,
        [this](const GeoPoseWithCovariance::SharedPtr msg) { on_geo_pose(msg); });

    // Publish pose topic
    pose_pub_ = create_publisher<PoseWithCovariance>("output_pose", 10);

    // Publish tf
    if (publish_tf_) {
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
        parent_frame_ = declare_parameter<std::string>("parent_frame");
        child_frame_ = declare_parameter<std::string>("child_frame");
    }
}

/// @brief 回调函数，当接收到地理位置姿态信息时被调用
/// @param msg geo地理位置姿态信息
void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg) {
    // 检查是否已经接收到了地图投影信
    if (!projector_info_) {
        RCLCPP_WARN_THROTTLE(get_logger(),
                             *get_clock(),
                             1000 /* ms */,
                             "map_projector_info is not received yet.");
        return;
    }

    // 接收到的地理位置姿态信息中提取出GPS坐标
    geographic_msgs::msg::GeoPoint gps_point;
    gps_point.latitude = msg->pose.pose.position.latitude;
    gps_point.longitude = msg->pose.pose.position.longitude;
    gps_point.altitude = msg->pose.pose.position.altitude;
    geometry_msgs::msg::Point position =
        geography_utils::project_forward(gps_point,
                                         projector_info_.value());  // 将GPS坐标转换投影到参考框架上
    position.z = geography_utils::convert_height(
        position.z,
        gps_point.latitude,
        gps_point.longitude,
        MapProjectorInfo::Message::WGS84,
        projector_info_.value().vertical_datum);  // 根据高程差异进行修正

    // Convert geo_pose to pose
    // 将这个投影后的位置信息转换为姿态信息格式
    PoseWithCovariance projected_pose;
    projected_pose.header = msg->header;
    projected_pose.pose.pose.position = position;
    projected_pose.pose.pose.orientation = msg->pose.pose.orientation;
    projected_pose.pose.covariance = msg->pose.covariance;

    // Covariance in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate.
    // TODO(TIER IV): This swap may be invalid when using other projector type.
    // 斜对角线上的元素交换位置，TODO：这个交换可能在使用其他投影类型时无效
    projected_pose.pose.covariance[0] = msg->pose.covariance[7];
    projected_pose.pose.covariance[7] = msg->pose.covariance[0];

    pose_pub_->publish(projected_pose);

    // Publish tf
    if (publish_tf_) {
        tf2::Transform transform;
        transform.setOrigin(tf2::Vector3(projected_pose.pose.pose.position.x,
                                         projected_pose.pose.pose.position.y,
                                         projected_pose.pose.pose.position.z));
        const auto localization_quat = tf2::Quaternion(projected_pose.pose.pose.orientation.x,
                                                       projected_pose.pose.pose.orientation.y,
                                                       projected_pose.pose.pose.orientation.z,
                                                       projected_pose.pose.pose.orientation.w);
        transform.setRotation(localization_quat);

        geometry_msgs::msg::TransformStamped transform_stamped;
        transform_stamped.header = msg->header;
        transform_stamped.header.frame_id = parent_frame_;
        transform_stamped.child_frame_id = child_frame_;
        transform_stamped.transform = tf2::toMsg(transform);
        tf_broadcaster_->sendTransform(transform_stamped);
    }
}

2. 带有IMU的里程计

这个里面也是只有一个gyro_odometer_core.cpp文件。这段代码是一个用于处理车辆速度和陀螺仪数据的节点，名为GyroOdometer。它包括了三个主要功能：transformCovariance函数用于对协方差矩阵进行变换，concatGyroAndOdometer函数用于合并车辆速度和陀螺仪数据，并对协方差进行处理，以及一些回调函数和发布函数。

在transformCovariance函数中，它通过计算输入协方差矩阵中对角线元素的最大值，然后将协方差矩阵的对角线元素设置为最大值，从而实现了对协方差矩阵的变换。

在concatGyroAndOdometer函数中，它首先计算车辆速度和陀螺仪数据的均值和原始协方差，然后根据观测数据的数量对原始协方差进行缩减，并构建一个TwistWithCovarianceStamped消息，填充其中的时间戳、坐标系、车辆速度和陀

…详情请参照古月居