FMT飞控接收位置消息

FMT飞控SIH模式通过MAVROS接收无人机位置消息

1. 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;
   }
   

2. 阅读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;
}