超维小课堂 | 7、ROS使用offboard模式控制无人机定点悬停源码分析

原创

已于 2024-06-14 20:04:41 修改 · 1.4k 阅读

27 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #ROS

于 2024-06-14 20:03:57 首次发布

引言：ROS使用offboard模式控制无人机进入定点悬停是学习ROS无人机控制的最经典的基本功能之一。基于此，本篇主要对此处的控制流程著一个简要的代码分析。（室内外通用代码）

顾名思义：offboard模式下的定点悬停是指通过机载电脑ROS发布指定的目标点，无人机按照程序发布的目标位置进行PID控制，到达后目标点后保持悬停不动。源码分析可以查看代码注释即可。

此处为视频详解：

源码视频详解：

特别注意：由于气压计以及GPS等定位存在漂移，会导致无人机在飞行过程中偏离程序设置的目标点。基于此，我们的代码已经对初始化漂移做了抵消，相对稳定，此处也是本此代码讲解的着重点。请确保使用我们的程序进行测试，否则可能造成无人机的失控

//包含ROS和MAVROS相关头文件 
#include <string> 
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>
#include <geometry_msgs/Twist.h>

//室外宏定义飞行高度2米，室内可适当降低高度
#define ALTITUDE  2

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
   
   
    current_state = *msg;
}

//定义变量，用于接收无人机的里程计和姿态信息
tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false;
nav_msgs::Odometry local_pos;
void local_pos_cb(const nav_msgs: