FMT飞控SIH模式通过MAVROS接收无人机位置消息
-
MAVROS获取位置消息的话题是
/mavros/local_position/pose
,理论上通过topic echo /mavros/local_position/pose
,就能获取到,但是我执行后没有收到消息。在ros中订阅话题可以用以下代码void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) { ROS_INFO("Received pose: [%f, %f, %f]", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); } int main(int argc, char** argv) { ros::init(argc, argv, "offboard__demo_figure_eight"); ros::NodeHandle nh; ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped》("/mavros/local_position/pose", 1000, poseCallback); return 0; }
-
阅读FMT文档可知,FMT默认只给机载电脑发送心跳包,如果需要接收其他的消息需要配置。
目前我们需要的是LOCAL_POSITION_NED这条消息,查询可知为32号包。
以下代码设置32号包的发送频率
#include <ros/ros.h>
#include <mavros_msgs/StreamRate.h>
int main(int argc, char **argv) {
ros::init(argc, argv, "offboard__demo_figure_eight");
ros::NodeHandle nh;
// for ID32 set rate for send in fmt
// 创建一个 /mavros/set_stream_rate 服务的客户端
ros::ServiceClient stream_rate_client =nh.serviceClient<mavros_msgs::StreamRate>("/mavros/set_stream_rate");
// 准备要更改频率的消息 ID 和频率
int message_id = 32;
int message_rate = 100;
// 创建一个服务请求对象
mavros_msgs::StreamRate srv;
srv.request.stream_id = message_id;
srv.request.message_rate = message_rate;
srv.request.on_off = true; // 启用消息发送
// 调用服务
if (stream_rate_client.call(srv)) {
ROS_INFO("Stream rate set successfully");
} else {
ROS_ERROR("Failed to set stream rate.");
}
return 0;
}