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

debug 15

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-29 14:39:39 +08:00
parent 7b52e3387c
commit 6b358671af
1 changed files with 51 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,11 +43,21 @@ class RtkAsensingNode : public rclcpp::Node
// 从 /Ins 消息中提取数据（通过 imu_msg 子字段）
rtk_msg.lat = msg->imu_msg.latitude;
rtk_msg.lon = msg->imu_msg.longitude;
rtk_msg.head = msg->imu_msg.azimuth;
// 航向角转换：将 azimuth 转换为 0~360 度（正北0，东90，南180，西270）
double heading = msg->imu_msg.azimuth;
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.p_quality = msg->imu_msg.position_type;
rtk_msg.h_quality = msg->imu_msg.heading_type;
// 定位质量从 /Gnss 获取，这里先用 /Ins 的数据作为备选
rtk_msg.p_quality = last_gnss_fix_type_;
rtk_msg.h_quality = last_gnss_heading_type_;
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);
@ -62,8 +72,40 @@ class RtkAsensingNode : public rclcpp::Node
void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg)
{
// GNSS数据可用于补充RTK信息（如果需要）
// 当前实现中RTK消息主要从/Ins获取
// 从 /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)
{
last_gnss_fix_type_ = 0; // 无效
}
else if (gps_fix == 1)
{
last_gnss_fix_type_ = 1; // GPS单点定位
}
else if (gps_fix == 2)
{
last_gnss_fix_type_ = 2; // DGPS差分定位
}
else if (gps_fix == 4 || gps_fix == 5)
{
last_gnss_fix_type_ = 4; // RTK固定解
}
else if (gps_fix == 6)
{
last_gnss_fix_type_ = 5; // RTK浮点解
}
else
{
last_gnss_fix_type_ = 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发布者
@ -72,6 +114,10 @@ class RtkAsensingNode : public rclcpp::Node
// 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)