前提
使用树莓派与pixhawk通信,安装好mavros,树莓派与pixhawk串口连接
启动节点mavros节点后,通过ros2 topic list
可以查看到一系列话题
查看话题的类型可以去wiki mavros中查看
或者使用ros2 topic info 话题名称
可以查看到
问题描述
- 订阅话题···/mavros/global_position/compass_hdg···
this->create_subscription<std_msg::msg::Float64>("/mavros/global_position/compass_hdg", 10, callbackfunc)
回调函数
void callbackfunc(const std_msg::msg::Float64::SharedPtr msg)
{cout << "callbackfunc" << endl;cout << "sub: " << msg->data << endl;
}
编译运行节点,无法触发回调函数,一句打印都没看到
使用 ros2 topic list
可以看到话题存在,但是订阅后无法获得消息
分析
使用 ros2 topic info /mavros/global_position/compass_hdg
查看
Type: std_msgs/msg/Float64
Publisher count: 1
Subscription count: 1
有1个订阅者,说明已经订阅上了,话题名称和话题类型已经对上
使用工具rqt_graph
查看节点之间的关系,包含了话题发布与订阅,在连接图上可以看到和预想的一致
自己写了个一模一样的话题发布出来,也可以订阅到,说明订阅者没有问题
原因
从发布者找问题,在rqt_graph图谱上可以查看发布的话题配置信息
其中QOS(服务质量)下的reliability: best_effort
Qos是ros2才有的,相关信息可参考这
reliability默认是reliable
既然发布话题配置了best_effort,订阅者也需要配置,否则读取不到信息
解决办法
10 -> rclcpp::SensorDataQoS()
订阅话题修改为
this->create_subscription<std_msg::msg::Float64>("/mavros/global_position/compass_hdg", rclcpp::SensorDataQoS(), callbackfunc)