diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp index fdbf072..fee895e 100644 --- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp +++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp @@ -5,7 +5,6 @@ #include "asensing_protocol.hpp" #include "imu_msgs/msg/gnss.hpp" -#include "imu_msgs/msg/imu.hpp" #include "logger/logger.h" #include "rclcpp/rclcpp.hpp" #include "sweeper_interfaces/msg/rtk.hpp" @@ -20,44 +19,42 @@ class RtkAsensingNode : public rclcpp::Node // 创建发布者 rtk_publisher_ = this->create_publisher("rtk_message", 10); - // 订阅 /Ins topic - ins_sub_ = this->create_subscription( - "/Ins", 10, std::bind(&RtkAsensingNode::ins_callback, this, std::placeholders::_1)); - - // 订阅 /Gnss topic (可选) + // 订阅 /Gnss topic gnss_sub_ = this->create_subscription( "/Gnss", 10, std::bind(&RtkAsensingNode::gnss_callback, this, std::placeholders::_1)); - LOG_INFO("已订阅 /Ins 和 /Gnss topic"); + LOG_INFO("已订阅 /Gnss topic"); } ~RtkAsensingNode() {} private: - void ins_callback(const imu_msgs::msg::Imu::SharedPtr msg) + void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg) { try { auto rtk_msg = sweeper_interfaces::msg::Rtk(); - // 从 /Ins 消息中提取数据(通过 imu_msg 子字段) - rtk_msg.lat = msg->imu_msg.latitude; - rtk_msg.lon = msg->imu_msg.longitude; + // 从 /Gnss 消息中获取数据 + rtk_msg.lat = msg->latitude; + rtk_msg.lon = msg->longitude; - // 航向角转换:将 azimuth 转换为 0~360 度(正北0,东90,南180,西270) - double heading = msg->imu_msg.azimuth; + // 航向角转换:将 yaw 转换为 0~360 度(正北0,东90,南180,西270) + double heading = msg->yaw; 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.speed = std::sqrt(msg->hor_vel * msg->hor_vel + msg->ver_vel * msg->ver_vel); - // 定位质量从 /Gnss 获取,这里先用 /Ins 的数据作为备选 - rtk_msg.p_quality = last_gnss_fix_type_; - rtk_msg.h_quality = last_gnss_heading_type_; + // 位置质量转换(使用 flags_pos 字段) + rtk_msg.p_quality = convertFixType(msg->flags_pos); + + // 定向质量转换(使用 flags_attitude 字段) + rtk_msg.h_quality = convertFixType(msg->flags_attitude); 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); @@ -66,58 +63,51 @@ class RtkAsensingNode : public rclcpp::Node } catch (const std::exception& e) { - LOG_ERROR("ins_callback异常:%s", e.what()); + LOG_ERROR("gnss_callback异常:%s", e.what()); } } - void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg) + // 将 Asensing fix 类型转换为 GPGGA 格式 + uint8_t convertFixType(uint8_t asensing_fix) { - // 从 /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) + // Asensing 类型定义: + // 0=无解, 1=位置固定, 8=多普勒速度, 16=单点定位, + // 17=伪距差分, 18=SBAS, 32=L1浮点, 34=窄巷浮点, + // 48=L1固定, 49=宽巷固定, 50=窄巷固定 + // + // GPGGA 格式: + // 0=无效, 1=GPS单点, 2=DGPS, 4=RTK固定, 5=RTK浮点 + if (asensing_fix == 0) { - last_gnss_fix_type_ = 0; // 无效 + return 0; // 无解 -> 无效 } - else if (gps_fix == 1) + else if (asensing_fix == 1 || asensing_fix == 16) { - last_gnss_fix_type_ = 1; // GPS单点定位 + return 1; // 位置固定/单点 -> GPS单点 } - else if (gps_fix == 2) + else if (asensing_fix == 17 || asensing_fix == 18 || asensing_fix == 8) { - last_gnss_fix_type_ = 2; // DGPS差分定位 + return 2; // 差分/SBAS/多普勒 -> DGPS } - else if (gps_fix == 4 || gps_fix == 5) + else if (asensing_fix == 32 || asensing_fix == 34) { - last_gnss_fix_type_ = 4; // RTK固定解 + return 5; // L1浮点/窄巷浮点 -> RTK浮点 } - else if (gps_fix == 6) + else if (asensing_fix == 48 || asensing_fix == 49 || asensing_fix == 50) { - last_gnss_fix_type_ = 5; // RTK浮点解 + return 4; // L1固定/宽巷固定/窄巷固定 -> RTK固定 } else { - last_gnss_fix_type_ = 0; // 其他视为无效 + return 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发布者 rclcpp::Publisher::SharedPtr rtk_publisher_; // ROS订阅者 - rclcpp::Subscription::SharedPtr ins_sub_; rclcpp::Subscription::SharedPtr gnss_sub_; - - // 缓存GNSS数据 - uint8_t last_gnss_fix_type_ = 0; - uint8_t last_gnss_heading_type_ = 0; }; int main(int argc, char** argv)