新增输出navsat/nmea_sentence_fix，低速模式下更改nmea原始数据。目前有问题

src/serial_ntrip_nmea.cpp

@@ -8,6 +8,7 @@
 #include <cstring>
 #include <cerrno>
 #include <vector>
+#include <deque>
 #include <thread>
 #include <atomic>
 #include <mutex>
@@ -17,6 +18,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nmea_msgs/msg/sentence.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include <cmath>
 
 class SerialNtripNmea : public rclcpp::Node
 {
@@ -47,6 +50,8 @@ public:
         
         // 创建NMEA消息发布器
         nmea_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>(nmea_topic_, 10);
+        // 额外创建一个带 RMC 航向修正的 NMEA 话题，保持原有话题行为不变
+        nmea_fix_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>("navsat/nmea_sentence_fix", 10);
         
         RCLCPP_INFO(this->get_logger(), "SerialNtripNmea初始化:");
         RCLCPP_INFO(this->get_logger(), "  串口: %s @ %d baud", serial_port_.c_str(), baud_rate_);
@@ -82,9 +87,20 @@ private:
     int serial_fd_{-1};
     int ntrip_socket_{-1};
     rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_pub_;
+    rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_fix_pub_;
     std::atomic<bool> running_{false};
     std::atomic<bool> ntrip_connected_{false};
     
+    // RMC 低速航向修正相关状态（仅用于 navsat/nmea_sentence_fix）
+    std::deque<double> rmc_heading_history_;
+    int low_speed_rmc_count_{0};
+    bool in_low_speed_mode_{false};
+    bool rmc_logic_activated_{false};
+    const double speed_threshold_kmh_{0.1};
+    // 进入低速时锁定当时的“往前第五条”航向，低速期间一直用该值，不每条更新
+    bool low_speed_heading_locked_{false};
+    double low_speed_locked_heading_{0.0};
+    
     bool setup_serial()
     {
         serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
@@ -261,7 +277,14 @@ private:
                             nmea_msg.header.stamp = this->now();
                             nmea_msg.sentence = nmea_buffer;
                             
+                            // 原始 NMEA 话题，行为保持不变
                             nmea_pub_->publish(nmea_msg);
+                            
+                            // 带 RMC 航向修正的 NMEA 话题
+                            auto nmea_msg_fix = nmea_msg;
+                            nmea_msg_fix.sentence = make_fixed_nmea_sentence(nmea_buffer);
+                            nmea_fix_pub_->publish(nmea_msg_fix);
+                            
                             RCLCPP_DEBUG(this->get_logger(), "发布NMEA消息: %s", nmea_buffer.c_str());
                             
                             // 检查是否为GGA语句，发送到NTRIP服务器
@@ -397,6 +420,161 @@ private:
         
         return encoded;
     }
+    
+    // 生成带 RMC 航向修正的 NMEA 语句，仅影响 navsat/nmea_sentence_fix 话题
+    std::string make_fixed_nmea_sentence(const std::string& sentence)
+    {
+        // 只处理以 $ 开头的 RMC 语句，其余直接原样返回
+        if (sentence.size() < 6 || sentence[0] != '$') {
+            return sentence;
+        }
+        
+        std::string type = sentence.substr(1, 5);
+        if (type != "GPRMC" && type != "GNRMC") {
+            return sentence;
+        }
+        
+        // 分离出不含校验和的部分（$ 与 * 之间）
+        std::size_t asterisk_pos = sentence.find('*');
+        std::string body_no_checksum;
+        if (asterisk_pos == std::string::npos) {
+            // 没有显式校验和时，直接去掉起始的 $
+            body_no_checksum = sentence.substr(1);
+        } else {
+            body_no_checksum = sentence.substr(1, asterisk_pos - 1);
+        }
+        
+        // 使用逗号分隔字段
+        std::vector<std::string> fields;
+        std::size_t start = 0;
+        while (true) {
+            std::size_t comma = body_no_checksum.find(',', start);
+            if (comma == std::string::npos) {
+                fields.emplace_back(body_no_checksum.substr(start));
+                break;
+            } else {
+                fields.emplace_back(body_no_checksum.substr(start, comma - start));
+                start = comma + 1;
+            }
+        }
+        
+        // RMC 标准字段中，索引 7 为地速（节），索引 8 为真航向
+        if (fields.size() <= 8) {
+            return sentence;
+        }
+        
+        bool speed_ok = false;
+        bool heading_ok = false;
+        double speed_knots = 0.0;
+        double heading_deg = 0.0;
+        
+        if (!fields[7].empty()) {
+            try {
+                speed_knots = std::stod(fields[7]);
+                speed_ok = true;
+            } catch (...) {
+                speed_ok = false;
+            }
+        }
+        
+        if (!fields[8].empty()) {
+            try {
+                heading_deg = std::stod(fields[8]);
+                heading_ok = true;
+            } catch (...) {
+                heading_ok = false;
+            }
+        }
+        
+        if (!heading_ok) {
+            // 航向字段无法解析时，不做任何改动
+            return sentence;
+        }
+        
+        // 将节转换成 km/h，并进行低速判定
+        bool is_low_speed = false;
+        double speed_kmh = 0.0;
+        if (speed_ok) {
+            speed_kmh = speed_knots * 1.852;
+            if (speed_kmh < speed_threshold_kmh_) {
+                is_low_speed = true;
+            }
+        }
+        
+        // 首次检测到地速 ≥ 阈值时，激活低速修正逻辑
+        if (!rmc_logic_activated_ && speed_ok && speed_kmh >= speed_threshold_kmh_) {
+            rmc_logic_activated_ = true;
+            low_speed_rmc_count_ = 0;
+            in_low_speed_mode_ = false;
+        }
+        
+        // 先计算“当前之前第五条”的航向（使用当前加入历史之前的记录），再写入历史
+        double fifth_prev_heading = heading_deg;
+        if (!rmc_heading_history_.empty() && rmc_heading_history_.size() >= 5) {
+            fifth_prev_heading = rmc_heading_history_[rmc_heading_history_.size() - 5];
+        }
+        
+        // 记录当前 RMC 航向到历史（用于后续“往前第五条”）
+        rmc_heading_history_.push_back(heading_deg);
+        if (rmc_heading_history_.size() > 50) {
+            rmc_heading_history_.pop_front();
+        }
+        
+        // 只有在逻辑被激活后，才根据低速连续次数进入/退出低速模式
+        // 进入低速时锁定当时的“往前第五条”航向，整个低速期间都用该值；退出时解除锁定
+        if (rmc_logic_activated_) {
+            if (is_low_speed) {
+                ++low_speed_rmc_count_;
+                if (low_speed_rmc_count_ >= 3) {
+                    bool just_entered = !in_low_speed_mode_;
+                    in_low_speed_mode_ = true;
+                    if (just_entered && rmc_heading_history_.size() >= 5) {
+                        low_speed_locked_heading_ = fifth_prev_heading;
+                        low_speed_heading_locked_ = true;
+                    }
+                }
+            } else {
+                low_speed_rmc_count_ = 0;
+                in_low_speed_mode_ = false;
+                low_speed_heading_locked_ = false;
+            }
+        }
+        
+        double output_heading = heading_deg;
+        if (in_low_speed_mode_ && low_speed_heading_locked_) {
+            output_heading = low_speed_locked_heading_;
+        }
+        
+        // 更新航向字段
+        {
+            std::ostringstream oss;
+            oss << std::fixed << std::setprecision(2) << output_heading;
+            fields[8] = oss.str();
+        }
+        
+        // 重新拼接不含校验和的部分
+        std::ostringstream body_oss;
+        for (std::size_t i = 0; i < fields.size(); ++i) {
+            if (i > 0) {
+                body_oss << ',';
+            }
+            body_oss << fields[i];
+        }
+        std::string new_body_no_checksum = body_oss.str();
+        
+        // 重新计算校验和（对 $ 与 * 之间的字符做 XOR）
+        unsigned char checksum = 0;
+        for (char ch : new_body_no_checksum) {
+            checksum ^= static_cast<unsigned char>(ch);
+        }
+        
+        std::ostringstream result;
+        result << '$' << new_body_no_checksum << '*'
+               << std::uppercase << std::hex << std::setw(2) << std::setfill('0')
+               << static_cast<int>(checksum);
+        
+        return result.str();
+    }
 };
 
 int main(int argc, char** argv)