订阅/cruise/wheel/twist判断静止模式。目前有问题

src/serial_ntrip_nmea.cpp

@@ -11,7 +11,6 @@
 #include <deque>
 #include <thread>
 #include <atomic>
-#include <mutex>
 #include <sstream>
 #include <iomanip>
 #include <signal.h>
@@ -52,6 +51,11 @@ public:
         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);
+
+        // 订阅 /cruise/wheel/twist 用于静止判定
+        twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
+            "/cruise/wheel/twist", 10,
+            std::bind(&SerialNtripNmea::twist_callback, this, std::placeholders::_1));
         
         RCLCPP_INFO(this->get_logger(), "SerialNtripNmea初始化:");
         RCLCPP_INFO(this->get_logger(), "  串口: %s @ %d baud", serial_port_.c_str(), baud_rate_);
@@ -88,18 +92,19 @@ private:
     int ntrip_socket_{-1};
     rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_pub_;
     rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_fix_pub_;
+    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
+    rclcpp::TimerBase::SharedPtr status_timer_;
     std::atomic<bool> running_{false};
     std::atomic<bool> ntrip_connected_{false};
-    
-    // RMC 低速航向修正相关状态（仅用于 navsat/nmea_sentence_fix）
+
+    // 静止判定：从 /cruise/wheel/twist 获取，linear.x==0 且 angular.z==0 为静止
+    std::atomic<bool> is_stationary_{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 in_stationary_mode_{false};
+    bool stationary_heading_locked_{false};
+    double stationary_locked_heading_{0.0};
     
     bool setup_serial()
     {
@@ -219,6 +224,14 @@ private:
         return false;
     }
     
+    void twist_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
+    {
+        constexpr double eps = 1e-6;
+        bool stationary = (std::fabs(msg->twist.linear.x) < eps) &&
+                          (std::fabs(msg->twist.angular.z) < eps);
+        is_stationary_.store(stationary);
+    }
+
     void start_server()
     {
         // 启动串口数据读取和NTRIP发送线程
@@ -229,12 +242,14 @@ private:
         std::thread ntrip_thread(&SerialNtripNmea::read_ntrip_and_write_serial, this);
         ntrip_thread.detach();
         
-        // 主循环 - 显示状态信息
-        rclcpp::Rate rate(1); // 1Hz
-        while (rclcpp::ok() && running_) {
-            RCLCPP_INFO(this->get_logger(), "运行状态 - NTRIP连接: %s", ntrip_connected_ ? "是" : "否");
-            rate.sleep();
-        }
+        // 使用定时器显示状态，使 spin() 能处理 twist 订阅回调
+        status_timer_ = this->create_wall_timer(
+            std::chrono::seconds(1),
+            [this]() {
+                if (rclcpp::ok() && running_) {
+                    RCLCPP_INFO(this->get_logger(), "运行状态 - NTRIP连接: %s", ntrip_connected_ ? "是" : "否");
+                }
+            });
     }
     
     void read_serial_and_process()
@@ -463,20 +478,9 @@ private:
             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]);
@@ -490,59 +494,34 @@ private:
             // 航向字段无法解析时，不做任何改动
             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;
+
+        // 静止判定：从 /cruise/wheel/twist 获取，linear.x==0 且 angular.z==0 为静止
+        bool stationary = is_stationary_.load();
+
+        // 进入/退出静止模式（在写入历史前判定，使用前10条那条数据的航向）
+        if (stationary) {
+            std::cout << "进入静止模式" << std::endl;
+            bool just_entered = !in_stationary_mode_;
+            in_stationary_mode_ = true;
+            if (just_entered && rmc_heading_history_.size() >= 10) {
+                stationary_locked_heading_ = rmc_heading_history_[rmc_heading_history_.size() - 10];
+                stationary_heading_locked_ = true;
             }
+        } else {
+            std::cout << "退出静止模式" << std::endl;
+            in_stationary_mode_ = false;
+            stationary_heading_locked_ = false;
         }
-        
-        // 首次检测到地速 ≥ 阈值时，激活低速修正逻辑
-        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 航向到历史（用于下次静止时取前10条）
         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_;
+        if (in_stationary_mode_ && stationary_heading_locked_) {
+            output_heading = stationary_locked_heading_;
         }
         
         // 更新航向字段