add publish new topic /cruise/LocationStatus and modify the conditions for publishing topic navsat/nmea_sentence_fix

CMakeLists.txt

@@ -22,7 +22,6 @@ find_package(ament_cmake_auto REQUIRED)
 # find_package(<dependency> REQUIRED)
 ament_auto_find_build_dependencies()
 
-
 ament_auto_add_executable(nmea_tcp src/nmea_tcp.cpp )
 ament_auto_add_executable(nmea_udp src/nmea_udp.cpp)
 ament_auto_add_executable(nmea_serial src/nmea_serial.cpp)

config/serial_ntrip_nmea_config.yaml

@@ -1,11 +1,11 @@
-serial_ntrip_nmea:
-  ros__parameters:
-    serial_port: "/dev/ttyUSB0"
-    baud_rate: 115200
-    ntrip_host: "120.253.226.97"  # 服务IP地址（备用IP: 120.253.239.161）
-    ntrip_port: 8001              # 服务端口
-    mountpoint: "RTCM33_GRCE"     # 接入点
-    username: "cwtm11609"          # CORS账号
-    password: "9hemate7"           # CORS密码
-    nmea_topic: "navsat/nmea_sentence"
-    frame_id: "sentence"
+serial_ntrip_nmea:
+  ros__parameters:
+    serial_port: "/dev/usb_gps"
+    baud_rate: 115200
+    ntrip_host: "103.143.19.54"  # 服务IP地址（备用IP: 120.253.239.161）
+    ntrip_port: 8002              # 服务端口
+    mountpoint: "RTCM33GRCEJpro"     # 接入点
+    username: "qxzckj5547"          # CORS账号
+    password: "34862"           # CORS密码
+    nmea_topic: "navsat/nmea_sentence"
+    frame_id: "sentence"

package.xml

@@ -9,9 +9,16 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>nmea_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>eagleye_msgs</depend>
+  <depend>cruise_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/serial_ntrip_nmea.cpp

@@ -11,13 +11,17 @@
 #include <deque>
 #include <thread>
 #include <atomic>
+#include <mutex>
 #include <sstream>
 #include <iomanip>
 #include <signal.h>
+#include <chrono>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nmea_msgs/msg/sentence.hpp"
+#include "cruise_msgs/msg/location_status.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
+#include "eagleye_msgs/msg/heading.hpp"
 #include <cmath>
 
 class SerialNtripNmea : public rclcpp::Node
@@ -49,6 +53,10 @@ public:
         
         // 创建NMEA消息发布器
         nmea_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>(nmea_topic_, 10);
+        nmea_status_pub_ = this->create_publisher<cruise_msgs::msg::LocationStatus>("/cruise/LocationStatus", 10);
+        nmea_status_timer_ = this->create_wall_timer(
+            std::chrono::seconds(1),
+            std::bind(&SerialNtripNmea::publish_nmea_status, this));
         // 额外创建一个带 RMC 航向修正的 NMEA 话题，保持原有话题行为不变
         nmea_fix_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>("navsat/nmea_sentence_fix", 10);
 
@@ -56,6 +64,10 @@ public:
         twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
             "/cruise/wheel/twist", 10,
             std::bind(&SerialNtripNmea::twist_callback, this, std::placeholders::_1));
+        // 订阅 /eagleye/heading_3rd，首次收到数据时启用静止航向修正
+        heading_3rd_sub_ = this->create_subscription<eagleye_msgs::msg::Heading>(
+            "/eagleye/heading_3rd", 10,
+            std::bind(&SerialNtripNmea::heading_3rd_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_);
@@ -91,16 +103,65 @@ private:
     int serial_fd_{-1};
     int ntrip_socket_{-1};
     rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_pub_;
+    rclcpp::Publisher<cruise_msgs::msg::LocationStatus>::SharedPtr nmea_status_pub_;
+    rclcpp::TimerBase::SharedPtr nmea_status_timer_;
     rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_fix_pub_;
     rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
+    rclcpp::Subscription<eagleye_msgs::msg::Heading>::SharedPtr heading_3rd_sub_;
     rclcpp::TimerBase::SharedPtr status_timer_;
     std::atomic<bool> running_{false};
     std::atomic<bool> ntrip_connected_{false};
+    std::atomic<int64_t> last_serial_rx_ms_{0};
+    std::atomic<int64_t> last_gga_valid_ms_{0};
+
+    static int64_t now_ms()
+    {
+        return std::chrono::duration_cast<std::chrono::milliseconds>(
+                   std::chrono::steady_clock::now().time_since_epoch())
+            .count();
+    }
+
+    bool gga_has_lat_lon(const std::string & nmea_sentence)
+    {
+        // GGA格式中: field[2]=latitude, field[4]=longitude
+        std::vector<std::string> fields;
+        std::stringstream ss(nmea_sentence);
+        std::string item;
+        while (std::getline(ss, item, ',')) {
+            fields.push_back(item);
+        }
+        if (fields.size() < 5) {
+            return false;
+        }
+        const std::string & lat = fields[2];
+        const std::string & lon = fields[4];
+        return !lat.empty() && !lon.empty() && lat != "0" && lon != "0";
+    }
+
+    void publish_nmea_status()
+    {
+        cruise_msgs::msg::LocationStatus status_msg;
+        status_msg.header.stamp = this->now();
+        status_msg.header.frame_id = frame_id_;
+
+        const int64_t now = now_ms();
+        const int64_t serial_gap_ms = now - last_serial_rx_ms_.load();
+        const int64_t gga_gap_ms = now - last_gga_valid_ms_.load();
+
+        status_msg.ntrip_account_ok = ntrip_connected_.load() ? 1 : 0;
+        status_msg.serial_rx_ok =
+            (last_serial_rx_ms_.load() > 0 && serial_gap_ms <= 3000) ? 1 : 0;
+        status_msg.gga_lat_lon_ok =
+            (last_gga_valid_ms_.load() > 0 && gga_gap_ms <= 3000) ? 1 : 0;
+
+        nmea_status_pub_->publish(status_msg);
+    }
 
     // 静止判定：从 /cruise/wheel/twist 获取，linear.x==0 且 angular.z==0 为静止
     std::atomic<bool> is_stationary_{false};
 
     // RMC 静止航向修正相关状态（仅用于 navsat/nmea_sentence_fix）
+    std::atomic<bool> stationary_heading_enabled_{false};  // 默认不启用，首次收到 /eagleye/heading_3rd 时启用
     std::deque<double> rmc_heading_history_;
     bool in_stationary_mode_{false};
     bool stationary_heading_locked_{false};
@@ -215,9 +276,11 @@ private:
                 RCLCPP_INFO(this->get_logger(), "NTRIP连接成功: %s", mountpoint_.c_str());
                 return true;
             } else {
+                ntrip_connected_ = false;
                 RCLCPP_ERROR(this->get_logger(), "NTRIP连接失败: %s", response.c_str());
             }
         } else {
+            ntrip_connected_ = false;
             RCLCPP_ERROR(this->get_logger(), "未收到NTRIP服务器响应");
         }
         
@@ -232,6 +295,13 @@ private:
         is_stationary_.store(stationary);
     }
 
+    void heading_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr /*msg*/)
+    {
+        if (!stationary_heading_enabled_.exchange(true)) {
+            RCLCPP_INFO(this->get_logger(), "收到 /eagleye/heading_3rd，已启用静止航向修正");
+        }
+    }
+
     void start_server()
     {
         // 启动串口数据读取和NTRIP发送线程
@@ -242,6 +312,13 @@ private:
         std::thread ntrip_thread(&SerialNtripNmea::read_ntrip_and_write_serial, this);
         ntrip_thread.detach();
         
+        // 主循环 - 显示状态信息
+        rclcpp::Rate rate(1); // 1Hz
+        while (rclcpp::ok() && running_) {
+            publish_nmea_status();
+            RCLCPP_INFO(this->get_logger(), "运行状态 - NTRIP连接: %s", ntrip_connected_ ? "是" : "否");
+            rate.sleep();
+        }
         // 使用定时器显示状态，使 spin() 能处理 twist 订阅回调
         status_timer_ = this->create_wall_timer(
             std::chrono::seconds(1),
@@ -262,6 +339,7 @@ private:
         while (rclcpp::ok() && running_) {
             ssize_t bytes_read = read(serial_fd_, buffer.data(), buffer.size());
             if (bytes_read > 0) {
+                last_serial_rx_ms_ = now_ms();
                 // 处理接收到的数据
                 for (ssize_t i = 0; i < bytes_read; ++i) {
                     char c = buffer[i];
@@ -324,6 +402,10 @@ private:
                                     }
                                 }
                             }
+
+                            if (is_gga && gga_has_lat_lon(nmea_buffer)) {
+                                last_gga_valid_ms_ = now_ms();
+                            }
                         }
                         
                         nmea_buffer.clear();
@@ -520,14 +602,14 @@ private:
         }
 
         double output_heading = heading_deg;
-        if (in_stationary_mode_ && stationary_heading_locked_) {
+        if (stationary_heading_enabled_.load() && in_stationary_mode_ && stationary_heading_locked_) {
             output_heading = stationary_locked_heading_;
         }
         
         // 更新航向字段
         {
             std::ostringstream oss;
-            oss << std::fixed << std::setprecision(2) << output_heading;
+            oss << std::fixed << std::setprecision(1) << output_heading;
             fields[8] = oss.str();
         }
         
@@ -566,4 +648,4 @@ int main(int argc, char** argv)
     rclcpp::spin(node);
     rclcpp::shutdown();
     return 0;
-}
+}