From 6b358671affc1ffe06aa8901c138fddf3112bd8b Mon Sep 17 00:00:00 2001
From: Alvin-lyq <zxwl@44.com>
Date: Wed, 29 Apr 2026 14:39:39 +0800
Subject: [PATCH] debug 15

---
 .../rtk_asensing/src/rtk_asensing_node.cpp    | 56 +++++++++++++++++--
 1 file changed, 51 insertions(+), 5 deletions(-)

diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp
index 9240ce6..fdbf072 100644
--- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp
+++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp
@@ -43,11 +43,21 @@ class RtkAsensingNode : public rclcpp::Node
             // 从 /Ins 消息中提取数据（通过 imu_msg 子字段）
             rtk_msg.lat = msg->imu_msg.latitude;
             rtk_msg.lon = msg->imu_msg.longitude;
-            rtk_msg.head = msg->imu_msg.azimuth;
+
+            // 航向角转换：将 azimuth 转换为 0~360 度（正北0，东90，南180，西270）
+            double heading = msg->imu_msg.azimuth;
+            if (heading < 0)
+            {
+                heading += 360.0;
+            }
+            rtk_msg.head = heading;
+
             rtk_msg.speed = std::sqrt(msg->imu_msg.north_velocity * msg->imu_msg.north_velocity +
                                        msg->imu_msg.east_velocity * msg->imu_msg.east_velocity);
-            rtk_msg.p_quality = msg->imu_msg.position_type;
-            rtk_msg.h_quality = msg->imu_msg.heading_type;
+
+            // 定位质量从 /Gnss 获取，这里先用 /Ins 的数据作为备选
+            rtk_msg.p_quality = last_gnss_fix_type_;
+            rtk_msg.h_quality = last_gnss_heading_type_;
 
             LOG_INFO("Asensing RTK: lat:%.9f, lon:%.9f, heading:%.2f, speed:%.2f, p_quality:%d, h_quality:%d",
                      rtk_msg.lat, rtk_msg.lon, rtk_msg.head, rtk_msg.speed, rtk_msg.p_quality, rtk_msg.h_quality);
@@ -62,8 +72,40 @@ class RtkAsensingNode : public rclcpp::Node
 
     void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg)
     {
-        // GNSS数据可用于补充RTK信息（如果需要）
-        // 当前实现中RTK消息主要从/Ins获取
+        // 从 /Gnss 消息中获取定位质量，转换为 GPGGA 格式
+        // GPGGA: 0=无效, 1=GPS单点, 2=DGPS, 4=RTK固定, 5=RTK浮点
+        uint8_t gps_fix = msg->gps_fix;
+
+        // 将 Asensing 的 gps_fix 转换为 GPGGA 格式
+        if (gps_fix == 0)
+        {
+            last_gnss_fix_type_ = 0;  // 无效
+        }
+        else if (gps_fix == 1)
+        {
+            last_gnss_fix_type_ = 1;  // GPS单点定位
+        }
+        else if (gps_fix == 2)
+        {
+            last_gnss_fix_type_ = 2;  // DGPS差分定位
+        }
+        else if (gps_fix == 4 || gps_fix == 5)
+        {
+            last_gnss_fix_type_ = 4;  // RTK固定解
+        }
+        else if (gps_fix == 6)
+        {
+            last_gnss_fix_type_ = 5;  // RTK浮点解
+        }
+        else
+        {
+            last_gnss_fix_type_ = 0;  // 其他视为无效
+        }
+
+        // 定向质量（如果有）
+        last_gnss_heading_type_ = msg->flags_attitude > 0 ? 4 : 0;
+
+        LOG_INFO("GNSS: gps_fix=%d -> p_quality=%d", gps_fix, last_gnss_fix_type_);
     }
 
     // ROS发布者
@@ -72,6 +114,10 @@ class RtkAsensingNode : public rclcpp::Node
     // ROS订阅者
     rclcpp::Subscription<imu_msgs::msg::Imu>::SharedPtr ins_sub_;
     rclcpp::Subscription<imu_msgs::msg::Gnss>::SharedPtr gnss_sub_;
+
+    // 缓存GNSS数据
+    uint8_t last_gnss_fix_type_ = 0;
+    uint8_t last_gnss_heading_type_ = 0;
 };
 
 int main(int argc, char** argv)