PX4自动控制中常用的几个话题

订阅话题
1、/mavros/state
订阅/发布：订阅
功能：订阅mavros的一些状态数据，如连接状态、是否解锁、当前无人机模式
数据类型：mavros_msgs/State

string MODE_PX4_MANUAL=MANUAL
string MODE_PX4_ACRO=ACRO
string MODE_PX4_ALTITUDE=ALTCTL
string MODE_PX4_POSITION=POSCTL
string MODE_PX4_OFFBOARD=OFFBOARD
string MODE_PX4_STABILIZED=STABILIZED
string MODE_PX4_RATTITUDE=RATTITUDE
string MODE_PX4_MISSION=AUTO.MISSION
string MODE_PX4_LOITER=AUTO.LOITER
string MODE_PX4_RTL=AUTO.RTL
string MODE_PX4_LAND=AUTO.LAND
string MODE_PX4_RTGS=AUTO.RTGS
string MODE_PX4_READY=AUTO.READY
string MODE_PX4_TAKEOFF=AUTO.TAKEOFF
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
bool connected
bool armed
bool guided
bool manual_input
string mode
uint8 system_status

使用示例：

// 依赖的库文件有：mavros、roscpp

// 1.头文件需要
#include <ros/ros.h>
#include <mavros_msgs/State.h>
// 2.回调函数，接受状态数据
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
	current_state = *msg;
}
// 3.订阅话题
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
	        ("/mavros/state",10,state_cb);
// 4.获取数据
// FCU有无连接上，bool
current_state.connected == true;
// FCU前飞行状态，string
current_state.mode == "OFFBOARD";
// FCU有无解锁，bool
current_state.armed == true;

2、/mavros/local_position/pose
订阅/发布：订阅
功能：话题内容为当前无人机坐标系在local世界坐标系的位姿
数据类型：geometry_msgs/PoseStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

使用示例：

// 依赖的库文件有：mavros、roscpp、geometry_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

double position[3] = {0,0,0};
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
	position[0] = msg->pose.position.x;
	position[1] = msg->pose.position.y;
	position[2] = msg->pose.position.z;
}
// 2.订阅话题
ros::Subscriber pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
			("/mavros/local_position/pose",10,pos_cb);

3、/mavros/local_position/velocity_local
订阅/发布：订阅

功能：话题内容为当前无人机的三轴速度，包括三轴线速度和三轴角速度，坐标系为local坐标系（以无人机上电点为原点、东北天朝向）

数据类型：geometry_msgs/TwistStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Twist twist
  geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z

使用示例：

// 依赖的库文件有：mavros、roscpp、geometry_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>

geometry_msgs::TwistStamped vel_msg;
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg){
	vel_msg = *msg;
}
// 2.订阅话题
ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
			("/mavros/local_position/velocity_local",10,vel_cb);
// 3.打印话题内容
std::cout << vel_msg.twist.linear.x << std::endl;
std::cout << vel_msg.twist.linear.y << std::endl;
std::cout << vel_msg.twist.linear.z << std::endl;

4、/mavros/imu/data(_raw)
订阅/发布：订阅

功能：话题内容为IMU九轴的数据（XYZ加速度、XYZ角速度、XYZ姿态角），data_raw为原始数据，data为滤波后的数据（有px4自行生成的四元数数据）。IMU坐标系为前左上body坐标系。

数据类型：sensor_msgs/Imu

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance

使用示例：

// 依赖的库文件有：mavros、roscpp、sensor_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

sensor_msgs::Imu imu_msg;
void imu_cb(const sensor_msgs::Imu::ConstPtr& msg){
	imu_msg = *msg;
}
// 2.订阅话题
ros::Subscriber vel_sub = nh.subscribe<sensor_msgs::Imu>
			("/mavros/imu/data",10,imu_cb);
// 3.打印话题内容
std::cout << imu_msg.linear_acceleration.x << std::endl;
std::cout << imu_msg.angular_velocity.x << std::endl;

发布话题
1、/mavros/setpoint_position/local
功能：发布指点飞行，当前坐标系为local当地坐标系，即以FCU上电为原点的ENU坐标系（东北天）

数据类型：geometry_msgs/PoseStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose pose
  geometry_msgs/Point position  //local 坐标系下的位置（xyz），只有 position 成员变量生效
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

使用示例：

// 依赖的库文件有：mavros、roscpp、geometry_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

// 2.订阅话题
ros::Publisher local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>
			("/mavros/setpoint_position/local",10);
// 3.创建位置数据变量，ENU坐标系
geometry_msgs::PoseStamped pose;
// 位置
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
// 姿态
pose.pose.orientation.w = 1;
pose.pose.orientation.x = 0;
pose.pose.orientation.y = 0;
pose.pose.orientation.z = 0;
// 4.发布定位话题
local_pose_pub.publish(pose);

2、/mavros/setpoint_velocity/cmd_vel_unstamped
订阅/发布：发布

功能：设置无人机线速度和角速度，坐标系为local坐标系（东北天）

数据类型：geometry_msgs/Twist

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

使用示例：

// 依赖的库文件有：mavros、roscpp、geometry_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

// 2.发布话题
ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>
			("/mavros/setpoint_velocity/cmd_vel_unstamped",10);
// 3.速度消息
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 1;
vel_msg.linear.y = 1;

// 4.发布话题
vel_pub.publish(vel_msg);
ros::spinOnce();

3、/mavros/setpoint_raw/attitude
功能：设置无人机姿态、角速度和推力

数据类型：mavros_msgs/AttitudeTarget

uint8 IGNORE_ROLL_RATE=1
uint8 IGNORE_PITCH_RATE=2
uint8 IGNORE_YAW_RATE=4
uint8 IGNORE_THRUST=64
uint8 IGNORE_ATTITUDE=128
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint8 type_mask
geometry_msgs/Quaternion orientation // 四元数姿态
  float64 x
  float64 y
  float64 z
  float64 w
geometry_msgs/Vector3 body_rate // 角速度，坐标系测试貌似是body坐标系
  float64 x
  float64 y
  float64 z
float32 thrust // 推力

使用示例：

// 依赖的库文件有：mavros、roscpp、geometry_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <mavros_msgs/AttitudeTarget.h>

// 2.订阅话题
ros::Publisher thrust_pub = nh.advertise<mavros_msgs::AttitudeTarget>
			("/mavros/setpoint_raw/attitude",10);
// 3.创建消息
mavros_msgs::AttitudeTarget thrust_msg;
thrust_msg.thrust = 0.7;
thrust_msg.body_rate.y = 1;

// 4.发布话题	
thrust_pub.publish(thrust_msg);
ros::spinOnce();

4、/mavros/setpoint_raw/local
功能：设置无人机目标位置
数据类型：mavros_msgs/PositionTarget

uint8 FRAME_LOCAL_NED=1
uint8 FRAME_LOCAL_OFFSET_NED=7
uint8 FRAME_BODY_NED=8
uint8 FRAME_BODY_OFFSET_NED=9
uint16 IGNORE_PX=1
uint16 IGNORE_PY=2
uint16 IGNORE_PZ=4
uint16 IGNORE_VX=8
uint16 IGNORE_VY=16
uint16 IGNORE_VZ=32
uint16 IGNORE_AFX=64
uint16 IGNORE_AFY=128
uint16 IGNORE_AFZ=256
uint16 FORCE=512
uint16 IGNORE_YAW=1024
uint16 IGNORE_YAW_RATE=2048
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint8 coordinate_frame
uint16 type_mask
geometry_msgs/Point position
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 velocity
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 acceleration_or_force
  float64 x
  float64 y
  float64 z
float32 yaw
float32 yaw_rate

使用示例

// 依赖的库文件有：mavros、roscpp、geometry_msgs

// 1.头文件需要
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/PositionTarget.h>
//创建话题
    ros::Publisher target_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local",10);

//3、创建消息
 mavros_msgs::PositionTarget position_home;
    position_home.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    position_home.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | 
                              mavros_msgs::PositionTarget::IGNORE_VY |
                              mavros_msgs::PositionTarget::IGNORE_VZ |
                              mavros_msgs::PositionTarget::IGNORE_AFX |
                              mavros_msgs::PositionTarget::IGNORE_AFY |
                              mavros_msgs::PositionTarget::IGNORE_AFZ |
                              mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
    position_home.position.x = 0;
    position_home.position.y = 0;
    position_home.position.z = FLIGHT_ALTITUDE;
    position_home.velocity.x = 0;
    position_home.velocity.y = 0;
    position_home.velocity.z = 0;
    position_home.acceleration_or_force.x = 0;
    position_home.acceleration_or_force.y = 0;
    position_home.acceleration_or_force.z = 0;
    position_home.yaw = (-45.0f + 90.0f) * PI / 180.0f;
    position_home.yaw_rate = 0;

    target_local_pub.publish(position_home);
    ros::spinOnce();
    rate.sleep();

常用的服务
1、/mavros/cmd/arming
Server端/Client端：Client端

功能：发布解锁/上锁命令

数据类型：mavros_msgs/CommandBool

bool value // true解锁,false上锁
---
bool success
uint8 result

使用示例：

// 依赖的库文件有：mavros、roscpp

// 1.头文件需要
#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>

// 2.订阅服务
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
			("/mavros/cmd/arming");
// 3.创建变量
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true; // true解锁,false上锁

// 4.请求服务
if( arming_client.call(arm_cmd) && arm_cmd.response.success){
	ROS_INFO("Vehicle armed");
}

2、/mavros/set_mode
Server端/Client端：Client端

功能：请求切换FCU的飞行模式

数据类型：mavros_msgs/SetMode
uint8 MAV_MODE_PREFLIGHT=0
uint8 MAV_MODE_STABILIZE_DISARMED=80
uint8 MAV_MODE_STABILIZE_ARMED=208
uint8 MAV_MODE_MANUAL_DISARMED=64
uint8 MAV_MODE_MANUAL_ARMED=192
uint8 MAV_MODE_GUIDED_DISARMED=88
uint8 MAV_MODE_GUIDED_ARMED=216
uint8 MAV_MODE_AUTO_DISARMED=92
uint8 MAV_MODE_AUTO_ARMED=220
uint8 MAV_MODE_TEST_DISARMED=66
uint8 MAV_MODE_TEST_ARMED=194
uint8 base_mode
// 常见的模式有MANUAL、ALTCTL、POSCTL、OFFBOARD、STABILIZED、AUTO.LAND

string custom_mode
---
bool mode_sent

使用示例：

// 依赖的库文件有：mavros、roscpp

// 1.头文件需要
#include <ros/ros.h>
#include <mavros_msgs/SetMode.h>

// 2.订阅服务
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
			("/mavros/set_mode");
// 3.创建变量
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

// 4.请求服务
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
	ROS_INFO("Offboard enabled");
}

！！！！！！！！！！真实环境测试时不要用软件切换offboard模式！！！！！！！！！！

！！！！！！！！！！真实环境测试时不要用软件切换offboard模式！！！！！！！！！！

！！！！！！！！！！真实环境测试时不要用软件切换offboard模式！！！！！！！！！！

特别是像例程那样反复在while循环中设置offboard模式，要是控制不了机载电脑关闭该节点的话飞机就永远保持offboard模式了，如果发出的还是速度指令的话就祈祷别砸到人吧。所以设置电子围栏也很重要。

飞机起飞前，不停刷新home点位置（可通过/mavros/home_position/home读取），该点位置和/mavros/local_position/pose历史位置数据一样，而不是/mavros/global_position/local话题，也就是说home点坐标是基于/mavros/local_position/pose话题的。因此要返航可以直接发布mavros/setpoint_raw/local话题，给出home点坐标即可。