[ROS] Basic knowledge and use of MAVROS

foreword

MAVLink is a lightweight communication protocol, mainly used to communicate between drones and ground stations, and contains many drone-related information and commands, such as drone status, sensor data, battery power etc. Some UAV hardware platforms such as Pixhawk, PX4, ArduPilot, etc. use MAVLink communication. MAVROS is an open source ROS package used to connect ROS and MAVLink protocol to realize communication and control between ROS and UAV. This post is updated from time to time to record my learning experience of MAVROS.

1. mavros installation

Mavros has two installation methods: source code installation and binary installation.

(1) Binary installation

# 1.安装mavros，根据自己的ros版本进行修改
sudo apt-get install ros-noetic-mavros ros-noetic-mavros-extras
# 2.安装GeographicLib依赖库安装
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh
# 3.测试安装是否成功
# 查看飞控端口
ll /dev/ttyACM*
# 添加权限
sudo chmod 777 /dev/ttyACM0
# 启动节点
roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0:57600

Two, mavros coordinate system

The mavros coordinate system is quite confusing, and it is different from the coordinate system commonly used by the FCU (flight control unit, so-called flight control) in PX4. The coordinate system used by the FCU of PX4 is NED (north east) or FRD (front lower right) coordinate system, but the coordinate system commonly used in mavros is ENU (northeast sky) or FLU (front left sky), mavros is converted to mavlink ENU will be automatically converted to NED when sending to FCU. Of course, we don't need to care about these, we only need to care about what coordinate system is needed when using mavros.

(1) The coordinate system is divided according to function

Common coordinate systems in the topic of mavros include global coordinate system, local coordinate system, and body coordinate system:

• The global system is easy to understand. It is a GPS coordinate system, such as latitude and longitude, which is not used much;

• local refers to the local coordinate system, usually the ENU coordinate system, and the coordinate origin is generally at the power-on point of PX4;

• The body coordinate system refers to the body coordinate system. The binary installed kinetic version of mavros has a coordinate system of the RFU (right front upper) coordinate system, and the origin of the coordinates is on the body. Later versions of mavros are uniformly changed to the FLU (front upper left) coordinate system;

(2) Coordinate system installation orientation division

• ENU coordinate system: northeast sky coordinate system, also called station center coordinate system, the local coordinate system in mavros generally adopts ENU coordinate system, such as topic /mavros/setpoint_position/local;

• NED coordinate system: North East coordinate system, also called navigation coordinate system, the NED coordinate system is used in the flight control FCU of PX4;

• FLU coordinate system: the front upper left coordinate system, that is, the UAV body coordinate system, with the UAV as the origin, is mostly used for the speed control of the UAV. For example, /mavros/setpoint_raw/localthe speed in the topic topic is the FLU coordinate system;

In addition, you can modify the coordinate system settings of the topic by modifying the following commands:

roscd mavros
cd launch/
sudo gedit px4_config.yaml

The coordinate system in the file mapis globalthe system and localthe system, and base_linkthe coordinate system is bodythe system. Then the coordinate system corresponding to each specific topic of mavros needs to be queried in detail.

3. Commonly used topics

（1）/mavros/state

• Subscribe/Publish: Subscribe

• Function: Subscribe to some status data of mavros, such as connection status, whether to unlock, current drone mode

• Data type: 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
  

• Example usage:

  // 依赖的库文件有：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/setpoint_position/local

• subscribe/publish:publish

• Function: release pointing flight, the current coordinate system is the local local coordinate system, that is, the ENU coordinate system (northeast sky) with the FCU powered on as the origin

• Data type: 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
  

• Example usage:

  // 依赖的库文件有：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);
  

• Supplementary note: The common coordinate systems in the topic of mavros include global system, local system, and body system.

  The global system is easy to understand. It is a GPS coordinate system, such as latitude and longitude, which is not used much.

  local refers to the local coordinate system, which refers to the local map or the world coordinate system, usually the ENU coordinate system, and the coordinate origin is generally where the PX4 is powered on.

  The body system refers to the coordinate system on the body, which is confusing. In the kinetic version of mavros, the body system is the RFU (right front upper) coordinate system, and the origin of the coordinates is on the body. In the melodic version and later versions, the body system has been changed to FLU (front upper left) coordinate system. (Front here refers to the direction of the nose of the aircraft)

（3）/mavros/local_position/pose

• Subscribe/Publish: Subscribe

• Function: The topic content is the pose of the current UAV coordinate system in the local world coordinate system. The local world coordinate system is based on the power-on point of the UAV PX4 as the origin, and the three-axis orientation is Northeast Heaven (ENU); UAV coordinates The system is the body coordinate system, and the orientation of the three axes is the front and upper left; explain the size of the accelerometer in the IMU, that is, the linear acceleration. The accelerometer considers the resultant force except gravity, that is to say, when the drone is in free fall , the acceleration of the Z-axis is 0. When the UAV is placed on the ground, the acceleration of the Z-axis is +g, because the UAV is supported by the ground at this time, and the direction is vertically upward, and the magnitude is g. If the Z-axis acceleration of the drone placed at rest is +g, it is considered that the Z-axis direction is downward (mistakenly thinking that g is gravity)

• Data type: 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
  

• Example usage:

  // 依赖的库文件有：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);
  
  

（4）/mavros/local_position/velocity_local

• Subscribe/Publish: Subscribe

• Function: The content of the topic is the current three-axis velocity of the drone, including the three-axis velocity and three-axis angular velocity, and the coordinate system is the local coordinate system (with the power-on point of the drone as the origin and the northeast direction)

• Data type: 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
  

• Example usage:

  // 依赖的库文件有：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;
  

（5）/mavros/imu/data(_raw)

• Subscribe/Publish: Subscribe

• Function: The topic content is the IMU nine-axis data (XYZ acceleration, XYZ angular velocity, XYZ attitude angle), data_raw is the original data, and data is the filtered data (the quaternion data generated by px4 itself ). The IMU coordinate system is the front upper left body coordinate system.

• Data type: 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
  

• Example usage:

  // 依赖的库文件有：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;
  

（6）/mavros/setpoint_accel/accel

• subscribe/publish:publish

• Function: Set the acceleration of the drone, but the effect is very poor, it is not recommended to use, if you want to control the acceleration, it is recommended to use the topic of controlling the thrust

• Data type: geometry_msgs/Vector3Stamped

  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Vector3 vector
    float64 x
    float64 y
    float64 z
  

（7）/mavros/setpoint_velocity/cmd_vel_unstamped

• subscribe/publish:publish

• Function: Set the linear velocity and angular velocity of the drone, and the coordinate system is the local coordinate system (northeast sky)

• Data type: geometry_msgs/Twist

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

• Example usage:

  // 依赖的库文件有：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();
  

（8）/mavros/setpoint_raw/attitude

• subscribe/publish:publish

• Function: Set UAV attitude, angular velocity and thrust

• Data type: 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 // 推力
  

• Example usage:

  // 依赖的库文件有：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. Commonly used services

（1）/mavros/cmd/arming

• Server/Client: Client

• Function: issue unlock/lock command

• Data type: mavros_msgs/CommandBool

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

• Example usage:

  // 依赖的库文件有：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

• Function: Request to switch the flight mode of the FCU

• Data type: 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
  

• Example usage:

  // 依赖的库文件有：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");
  }
  

• Note: Be careful to switch to OFFBOARD mode, especially in actual flight, it is better to use the remote control to switch OFFBOARD instead of using the code to switch it yourself, especially in the while loop to switch to OFFBOARD, once an accident occurs, you need to kill this node to prevent others Switch the OFFBOARD mode, otherwise your remote control cannot switch out the OFFBOARD, only the handbrake