请教下，p230uwb版本，使用prometheus如何获取uwb定位数据？

古刹梵音

目前使用下述代码，未能获取到UWB数据，请教下获取UWB数据的思路// 新增UWB订阅者
ros::Subscriber uwb_sub = n.subscribe<prometheus_msgs::LinktrackNodeframe2>("/nlink_linktrack_nodeframe2", 10, uwb_cb);


// 新增 UWB 回调函数
void uwb_cb(const prometheus_msgs::LinktrackNodeframe2::ConstPtr &msg)
{
    uwb_data = *msg;
    std::cout << "uwb_cb" std::endl;
}

std::cout << "UWB: (" << uwb_data.pos_3d[0] << ", " << uwb_data.pos_3d[1] << ", " << uwb_data.pos_3d[2] << ")" << std::endl;






飞机实时状态数据如下
Header: 1746611225.979738251
UAV ID: +1
Connected: +0
Armed: +0
Mode:
Location Source: +6
Odom Valid: +0
GPS Status: +0
GPS Num: +0
Position: (+0.00, -0.01, +0.06)
Range: +0.06
Latitude: +30.79
Longitude: +103.86
Altitude: +100.00
Relative Alt: +0.00
Velocity: (-0.00, -0.00, -0.00)
Attitude: (+0.01, +0.01, +1.47)
Attitude Q: (+0.00, -0.01, -0.67, -0.74)
Attitude Rate: (+0.00, +0.00, +0.00)
Battery State: +15.23
Battery Percentage: +0.35
UWB: (+0.00, +0.00, +0.00)

蓝白调已获得悬赏

最佳答案

如果你是买的uwb版本按照手册来就可以，如果是自己安装uwb，首先按照uwb手册让uwb正常工作，在p230机载电脑中安装uwb ros驱动，运行ros程序，生成uwb 定位ros话题，默认我们uwb话题订阅如下： uwb_sub = nh.subscrib ...

• 蓝白调
• 发表于2025-05-08
• 详细答案 >

蓝白调

如果你是买的uwb版本按照手册来就可以，如果是自己安装uwb，首先按照uwb手册让uwb正常工作，在p230机载电脑中安装uwb ros驱动，运行ros程序，生成uwb 定位ros话题，默认我们uwb话题订阅如下： uwb_sub = nh.subscribe<prometheus_msgs::LinktrackNodeframe2>("/nlink_linktrack_nodeframe2", 10, &UAV_estimator::uwb_cb, this);回调函数如下：void UAV_estimator::uwb_cb(const prometheus_msgs::LinktrackNodeframe2::ConstPtr &msg)
{
    pos_drone_uwb[0] = msg->pos_3d[0];
    pos_drone_uwb[1] = msg->pos_3d[1];
    pos_drone_uwb[2] = msg->pos_3d[2];
    q_uwb = Eigen::Quaterniond(msg->quaternion[0],msg->quaternion[1],msg->quaternion[2],msg->quaternion[3]);
    Euler_uwb = quaternion_to_euler(q_uwb);

    get_uwb_stamp = ros::Time::now(); // 记录时间戳，防止超时
}，然后赋值给vision pose：else if (location_source == prometheus_msgs::UAVState::UWB)
    {
        //j作为计数变量，初始值为0，这里循环6次为了排除初始UWB位置值不准，以第6次为准，测试下来在LinkTrack S型号UWB是没有问题的
        if(j<6)
        {
        
        uwb_offset.x = pos_drone_uwb[0];
        uwb_offset.y = pos_drone_uwb[1];
        j+=1;
        
        }

        vision_pose.header = uav_state.header;
        vision_pose.header.stamp = ros::Time::now();
        // vision_pose = uwb_pose;
        //这里如果不减去初始位置offset值，按照UWB的坐标系，地面电脑A0基站位置是原点，用户可能产生混乱
        //因为无人机基本都是按照上电位置为原点（0，0，0），所以这样减去。如果不需要，那么不减去直接赋值UWB就可以了
        //这里之所以只针对xy做offset偏移，z不做，是因为在LinkTrack S下z轴高度数据是激光雷达提供
        vision_pose.pose.position.x = pos_drone_uwb[0] - uwb_offset.x;
        vision_pose.pose.position.y = pos_drone_uwb[1] - uwb_offset.y;
        vision_pose.pose.position.z = uav_state.range;

        vision_pose.pose.orientation.x = q_uwb.x();
        vision_pose.pose.orientation.y = q_uwb.y();
        vision_pose.pose.orientation.z = q_uwb.z();
        vision_pose.pose.orientation.w = q_uwb.w();
    }这些在我们对外代码中都可以找到：https://gitee.com/amovlab/Prometheus