通过gps数据发布odom(map中base_footprint)的位置

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <car_interfaces/GpsImuInterface.h>

// 用于存储初始化位置和姿态
struct Pose {
    double x, y, z;
    double roll, pitch, yaw;
};

// 全局变量
Pose init_location_, current_pose_;
bool loc_sub_ = false;  // 用于标记是否接收到GPS信息
bool init_ = true;      // 用于标记是否初始化完成

// 发布静态 world->map 的 transform
void publishStaticWorldToMap(const geometry_msgs::TransformStamped& base_footprint_in_world)
{
    static tf2_ros::StaticTransformBroadcaster static_broadcaster;
    geometry_msgs::TransformStamped world_to_map;

    // world_to_map 的时间戳和父子坐标系名称
    world_to_map.header.stamp = ros::Time::now();
    world_to_map.header.frame_id = "world";
    world_to_map.child_frame_id = "map";

    // 位置设置为 base_footprint 在 world 的逆
    world_to_map.transform.translation.x = base_footprint_in_world.transform.translation.x;
    world_to_map.transform.translation.y = base_footprint_in_world.transform.translation.y;
    world_to_map.transform.translation.z = base_footprint_in_world.transform.translation.z;

    // 设置姿态
    world_to_map.transform.rotation = base_footprint_in_world.transform.rotation;

    // 发布静态 world -> map 的 transform
    static_broadcaster.sendTransform(world_to_map);
}

// 发布动态的 map 到 base_footprint 的 transform
void publishDynamicMapToBase(const geometry_msgs::TransformStamped& base_footprint_in_map)
{
    static tf2_ros::TransformBroadcaster tf_broadcaster;
    geometry_msgs::TransformStamped map_to_base;

    // 设置时间戳和父子坐标系
    map_to_base.header.stamp = ros::Time::now();
    map_to_base.header.frame_id = "map";
    map_to_base.child_frame_id = "base_footprint";

    // 设置位置
    map_to_base.transform.translation.x = base_footprint_in_map.transform.translation.x;
    map_to_base.transform.translation.y = base_footprint_in_map.transform.translation.y;
    map_to_base.transform.translation.z = base_footprint_in_map.transform.translation.z;

    // 设置姿态
    map_to_base.transform.rotation = base_footprint_in_map.transform.rotation;

    // 发布动态的 transform
    tf_broadcaster.sendTransform(map_to_base);
}

// 发布 odom 话题
void publishOdom(const tf2::Transform& base_footprint_in_map, ros::Publisher& odom_pub)
{
    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "map";  // 使用 map 坐标系

    // 设置位置
    odom.pose.pose.position.x = base_footprint_in_map.getOrigin().x();
    odom.pose.pose.position.y = base_footprint_in_map.getOrigin().y();
    odom.pose.pose.position.z = base_footprint_in_map.getOrigin().z();

    // 设置姿态
    odom.pose.pose.orientation.x = base_footprint_in_map.getRotation().x();
    odom.pose.pose.orientation.y = base_footprint_in_map.getRotation().y();
    odom.pose.pose.orientation.z = base_footprint_in_map.getRotation().z();
    odom.pose.pose.orientation.w = base_footprint_in_map.getRotation().w();

    // 发布 odom 话题
    odom_pub.publish(odom);
}

// GPS/IMU 回调函数，更新全局位置和姿态
void locationCallback(const car_interfaces::GpsImuInterface::ConstPtr& msg)
{
    loc_sub_ = true;
    if (init_)
    {
        // 只初始化一次
        init_location_.x = msg->x;
        init_location_.y = msg->y;
        init_location_.z = msg->z;
        init_location_.roll = msg->roll * M_PI / 180;
        init_location_.pitch = msg->pitch * M_PI / 180;
        init_location_.yaw = msg->yaw * M_PI / 180;
        init_ = false;
    }
    
    // 更新当前姿态
    current_pose_.x = msg->x;
    current_pose_.y = msg->y;
    current_pose_.z = msg->z;
    current_pose_.roll = msg->roll * M_PI / 180;
    current_pose_.pitch = msg->pitch * M_PI / 180;
    current_pose_.yaw = msg->yaw * M_PI / 180;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf_and_odom_publisher");
    ros::NodeHandle nh;

    // 订阅 GPS/IMU 数据
    ros::Subscriber loc_sub = nh.subscribe("/gps_imu", 10, locationCallback);

    // 发布 odom 话题
    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

    ros::Rate rate(100);
    
    // 等待GPS消息订阅
    while (ros::ok() && init_)
    {
        ros::spinOnce();
        rate.sleep();
    }

    // 构建 base_footprint 在 world 的初始 Transform
    tf2::Quaternion q_init;
    q_init.setRPY(init_location_.roll, init_location_.pitch, init_location_.yaw);

    geometry_msgs::TransformStamped base_footprint_in_world;
    base_footprint_in_world.header.stamp = ros::Time::now();
    base_footprint_in_world.header.frame_id = "world";
    base_footprint_in_world.child_frame_id = "base_footprint";
    base_footprint_in_world.transform.translation.x = init_location_.x;
    base_footprint_in_world.transform.translation.y = init_location_.y;
    base_footprint_in_world.transform.translation.z = init_location_.z;
    base_footprint_in_world.transform.rotation.x = q_init.x();
    base_footprint_in_world.transform.rotation.y = q_init.y();
    base_footprint_in_world.transform.rotation.z = q_init.z();
    base_footprint_in_world.transform.rotation.w = q_init.w();

    while (ros::ok())
    {
        // 发布静态 world -> map 的 transform
        publishStaticWorldToMap(base_footprint_in_world);
        
        // base_footprint 在 map 坐标系中的变化
        geometry_msgs::TransformStamped base_footprint_in_map;
        base_footprint_in_map.transform.translation.x = current_pose_.x - init_location_.x;
        base_footprint_in_map.transform.translation.y = current_pose_.y - init_location_.y;
        base_footprint_in_map.transform.translation.z = current_pose_.z - init_location_.z;
        tf2::Quaternion q_pose;
        q_pose.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw);
        base_footprint_in_map.transform.rotation = tf2::toMsg(q_pose);

        // 发布动态的 map -> base_footprint 的 transform
        publishDynamicMapToBase(base_footprint_in_map);
        
        // 计算 base_footprint 在 map 坐标系中的位置
        tf2::Transform map_to_base;
        map_to_base.setOrigin(tf2::Vector3(base_footprint_in_map.transform.translation.x,
                                           base_footprint_in_map.transform.translation.y,
                                           base_footprint_in_map.transform.translation.z));
        map_to_base.setRotation(q_pose);
        
        // 发布 odom 数据 (base_footprint 在 map 中的位置)
        publishOdom(map_to_base, odom_pub);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}