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 "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<sweeper_interfaces::msg::Rtk>("rtk_message", 10);
// 订阅 /Ins topic
ins_sub_ = this->create_subscription<imu_msgs::msg::Imu>(
"/Ins", 10, std::bind(&RtkAsensingNode::ins_callback, this, std::placeholders::_1));
// 订阅 /Gnss topic (可选)
// 订阅 /Gnss topic
gnss_sub_ = this->create_subscription<imu_msgs::msg::Gnss>(
"/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<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_publisher_;
// ROS订阅者
rclcpp::Subscription<imu_msgs::msg::Imu>::SharedPtr ins_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)