lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug 16

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-29 15:02:46 +08:00
parent 6b358671af
commit d75337560a
1 changed files with 36 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
src/perception/rtk_asensing/src/rtk_asensing_node.cpp
View File

@ -5,7 +5,6 @@
#include "asensing_protocol.hpp" #include "asensing_protocol.hpp"
#include "imu_msgs/msg/gnss.hpp" #include "imu_msgs/msg/gnss.hpp"
#include "imu_msgs/msg/imu.hpp"
#include "logger/logger.h" #include "logger/logger.h"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/rtk.hpp"
@ -20,44 +19,42 @@ class RtkAsensingNode : public rclcpp::Node
// 创建发布者 // 创建发布者
rtk_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10); rtk_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10);
// 订阅 /Ins topic // 订阅 /Gnss topic
ins_sub_ = this->create_subscription<imu_msgs::msg::Imu>(
"/Ins", 10, std::bind(&RtkAsensingNode::ins_callback, this, std::placeholders::_1));
// 订阅 /Gnss topic (可选)
gnss_sub_ = this->create_subscription<imu_msgs::msg::Gnss>( gnss_sub_ = this->create_subscription<imu_msgs::msg::Gnss>(
"/Gnss", 10, std::bind(&RtkAsensingNode::gnss_callback, this, std::placeholders::_1)); "/Gnss", 10, std::bind(&RtkAsensingNode::gnss_callback, this, std::placeholders::_1));
LOG_INFO("已订阅 /Ins 和 /Gnss topic"); LOG_INFO("已订阅 /Gnss topic");
} }
~RtkAsensingNode() {} ~RtkAsensingNode() {}
private: private:
void ins_callback(const imu_msgs::msg::Imu::SharedPtr msg) void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg)
{ {
try try
{ {
auto rtk_msg = sweeper_interfaces::msg::Rtk(); auto rtk_msg = sweeper_interfaces::msg::Rtk();
// 从 /Ins 消息中提取数据（通过 imu_msg 子字段） // 从 /Gnss 消息中获取数据
rtk_msg.lat = msg->imu_msg.latitude; rtk_msg.lat = msg->latitude;
rtk_msg.lon = msg->imu_msg.longitude; rtk_msg.lon = msg->longitude;
// 航向角转换：将 azimuth 转换为 0~360 度（正北0，东90，南180，西270） // 航向角转换：将 yaw 转换为 0~360 度（正北0，东90，南180，西270）
double heading = msg->imu_msg.azimuth; double heading = msg->yaw;
if (heading < 0) if (heading < 0)
{ {
heading += 360.0; heading += 360.0;
} }
rtk_msg.head = heading; 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 的数据作为备选 // 位置质量转换（使用 flags_pos 字段）
rtk_msg.p_quality = last_gnss_fix_type_; rtk_msg.p_quality = convertFixType(msg->flags_pos);
rtk_msg.h_quality = last_gnss_heading_type_;
// 定向质量转换（使用 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", 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); 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) 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 格式 // Asensing 类型定义:
// GPGGA: 0=无效, 1=GPS单点, 2=DGPS, 4=RTK固定, 5=RTK浮点 // 0=无解, 1=位置固定, 8=多普勒速度, 16=单点定位,
uint8_t gps_fix = msg->gps_fix; // 17=伪距差分, 18=SBAS, 32=L1浮点, 34=窄巷浮点,
// 48=L1固定, 49=宽巷固定, 50=窄巷固定
// 将 Asensing 的 gps_fix 转换为 GPGGA 格式 //
if (gps_fix == 0) // 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 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发布者 // ROS发布者
rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_publisher_; rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_publisher_;
// ROS订阅者 // ROS订阅者
rclcpp::Subscription<imu_msgs::msg::Imu>::SharedPtr ins_sub_;
rclcpp::Subscription<imu_msgs::msg::Gnss>::SharedPtr gnss_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) int main(int argc, char** argv)