|
|
发表于 前天 17:48
86 浏览 1 回复
请教下，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 ...
|
扫一扫浏览分享
|
|
|
|
|
|
|
|
|
|
|
发表于 昨天 09:38
如果你是买的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 |
|
|
|
|
|
|
|
|
|
|
