[ROS] Conocimientos básicos y uso de MAVROS

prefacio

MAVLink es un protocolo de comunicación liviano, que se utiliza principalmente para comunicarse entre drones y estaciones terrestres, y contiene mucha información y comandos relacionados con los drones, como el estado del dron, los datos del sensor, la energía de la batería, etc. Algunas plataformas de hardware UAV como Pixhawk, PX4, ArduPilot, etc. utilizan la comunicación MAVLink. MAVROS es un paquete ROS de código abierto que se utiliza para conectar el protocolo ROS y MAVLink para realizar la comunicación y el control entre ROS y UAV. Esta publicación se actualiza de vez en cuando para registrar mi experiencia de aprendizaje de MAVROS.

1. instalación mavros

Mavros tiene dos métodos de instalación: instalación de código fuente e instalación binaria.

(1) Instalación binaria

# 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

Dos, sistema de coordenadas mavros

El sistema de coordenadas mavros es bastante confuso y es diferente del sistema de coordenadas comúnmente utilizado por la FCU (unidad de control de vuelo, también llamada control de vuelo) en PX4. El sistema de coordenadas utilizado por la FCU de PX4 es el sistema de coordenadas NED (noreste) o FRD (frontal inferior derecho), pero el sistema de coordenadas comúnmente utilizado en mavros es ENU (cielo nororiental) o FLU (cielo frontal izquierdo), mavros se convierte a mavlink ENU se convertirá automáticamente a NED cuando se envíe a FCU. Por supuesto, no necesitamos preocuparnos por estos, solo debemos preocuparnos por qué sistema de coordenadas se necesita al usar mavros.

(1) El sistema de coordenadas se divide según la función

Los sistemas de coordenadas comunes en el tema de mavros incluyen el sistema de coordenadas global, el sistema de coordenadas local y el sistema de coordenadas del cuerpo:

• El sistema global es fácil de entender, es un sistema de coordenadas GPS, como latitud y longitud, que no se usa mucho;

• local se refiere al sistema de coordenadas local, generalmente el sistema de coordenadas ENU, y el origen de coordenadas generalmente se encuentra en el punto de encendido de PX4;

• El sistema de coordenadas del cuerpo se refiere al sistema de coordenadas del cuerpo. La versión cinética binaria instalada de mavros tiene un sistema de coordenadas del sistema de coordenadas RFU (superior frontal derecho), y el origen de las coordenadas está en el cuerpo. Las versiones posteriores de mavros se cambian uniformemente al sistema de coordenadas FLU (frente superior izquierdo);

(2) División de orientación de la instalación del sistema de coordenadas

• Sistema de coordenadas ENU: sistema de coordenadas del cielo del noreste, también llamado sistema de coordenadas del centro de la estación, el sistema de coordenadas local en mavros generalmente adopta el sistema de coordenadas ENU, como el tema /mavros/setpoint_position/local;

• Sistema de coordenadas NED: sistema de coordenadas del noreste, también llamado sistema de coordenadas de navegación, el sistema de coordenadas NED se utiliza en la FCU de control de vuelo de PX4;

• Sistema de coordenadas FLU: el sistema de coordenadas frontal superior izquierdo, es decir, el sistema de coordenadas del cuerpo del UAV, con el UAV como origen, se usa principalmente para el control de velocidad del UAV. Por ejemplo, la velocidad en el tema es la /mavros/setpoint_raw/localFLU sistema coordinado;

Además, puede modificar la configuración del sistema de coordenadas del tema modificando los siguientes comandos:

roscd mavros
cd launch/
sudo gedit px4_config.yaml

El sistema de coordenadas en el archivo mapes globalel sistema y localel sistema, y base_link​​el sistema de coordenadas es bodyel sistema. Luego, el sistema de coordenadas correspondiente a cada tema específico de mavros debe consultarse en detalle.

3. Temas de uso común

(1)/mavros/estado

• Suscribirse/Publicar: Suscribirse

• Función: suscríbase a algunos datos de estado de mavros, como el estado de conexión, si desbloquear, modo de dron actual

• Tipo de datos: mavros_msgs/Estado

  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
  

• Ejemplo de uso:

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

• suscribirse/publicar:publicar

• Función: lanzamiento de vuelo apuntando, el sistema de coordenadas actual es el sistema de coordenadas local local, es decir, el sistema de coordenadas ENU (cielo nororiental) con la FCU encendida como origen

• Tipo de datos: geometric_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
  

• Ejemplo de uso:

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

• Nota complementaria: Los sistemas de coordenadas comunes en el tema de mavros incluyen el sistema global, el sistema local y el sistema corporal.

  El sistema global es fácil de entender, es un sistema de coordenadas GPS, como latitud y longitud, que no se usa mucho.

  local se refiere al sistema de coordenadas local, que se refiere al mapa local o al sistema de coordenadas mundial, generalmente el sistema de coordenadas ENU, y el origen de coordenadas generalmente es donde se enciende el PX4.

  El sistema del cuerpo se refiere al sistema de coordenadas en el cuerpo, lo cual es confuso. Para la versión cinética de mavros, el sistema del cuerpo es el sistema de coordenadas RFU (superior frontal derecho), y el origen de las coordenadas está en el cuerpo. versión melódica y versiones posteriores, el sistema del cuerpo se ha cambiado al sistema de coordenadas FLU (frente superior izquierdo). (Frente aquí se refiere a la dirección de la nariz de la aeronave)

(3)/mavros/local_position/pose

• Suscribirse/Publicar: Suscribirse

• Función: El contenido del tema es la pose del sistema de coordenadas UAV actual en el sistema de coordenadas del mundo local. El sistema de coordenadas del mundo local se basa en el punto de encendido del UAV PX4 como origen, y la orientación de tres ejes es Noreste. Heaven (ENU); coordenadas UAV El sistema es el sistema de coordenadas del cuerpo, y la orientación de los tres ejes es la parte delantera y superior izquierda; explique el tamaño del acelerómetro en la IMU, es decir, la aceleración lineal. El acelerómetro considera la fuerza resultante excepto la gravedad, es decir, cuando el dron está en caída libre, la aceleración del eje Z es 0. Cuando el dron está colocado en el suelo, la aceleración del eje Z es +g, porque el dron está sostenido por el suelo en este momento, la dirección es verticalmente hacia arriba y la magnitud es g, así que no lo hagas solo porque Si la aceleración del eje Z del dron colocado en reposo es +g, se considera que el eje Z la dirección es hacia abajo (pensando erróneamente que g es la gravedad)

• Tipo de datos: geometric_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
  

• Ejemplo de uso:

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

• Suscribirse/Publicar: Suscribirse

• Función: el contenido del tema es la velocidad actual de tres ejes del dron, incluida la velocidad de tres ejes y la velocidad angular de tres ejes, y el sistema de coordenadas es el sistema de coordenadas local (con el punto de encendido del dron como el origen y la dirección noreste)

• Tipo de datos: geometric_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
  

• Ejemplo de uso:

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

• Suscribirse/Publicar: Suscribirse

• Función: el contenido del tema son los datos de nueve ejes de IMU (aceleración XYZ, velocidad angular XYZ, ángulo de actitud XYZ), data_raw son los datos originales y data son los datos filtrados (los datos de cuaterniones generados por el propio px4 ) . El sistema de coordenadas IMU es el sistema de coordenadas del cuerpo frontal superior izquierdo.

• Tipo de datos: 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
  

• Ejemplo de uso:

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

• suscribirse/publicar:publicar

• Función: establece la aceleración del dron, pero el efecto es muy pobre, no se recomienda su uso, si desea controlar la aceleración, se recomienda utilizar el tema de control de empuje

• Tipo de datos: geometric_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

• suscribirse/publicar:publicar

• Función: establece la velocidad lineal y la velocidad angular del dron, y el sistema de coordenadas es el sistema de coordenadas local (cielo nororiental)

• Tipo de datos: geometric_msgs/Twist

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

• Ejemplo de uso:

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

• suscribirse/publicar:publicar

• Función: establece la actitud, la velocidad angular y el empuje del UAV

• Tipo de datos: 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 // 推力
  

• Ejemplo de uso:

  // 依赖的库文件有：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. Servicios de uso común

(1)/mavros/cmd/armado

• Servidor/Cliente: Cliente

• Función: emitir comando de desbloqueo/bloqueo

• Tipo de datos: mavros_msgs/CommandBool

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

• Ejemplo de uso:

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

• Servidor/Cliente: Cliente

• Función: solicitud para cambiar el modo de vuelo de la FCU

• Tipo de datos: 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
  

• Ejemplo de uso:

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

• Nota: tenga cuidado de cambiar al modo FUERA DE BORDO, especialmente en el vuelo real, es mejor usar el control remoto para cambiar el modo FUERA DE BORDA en lugar de usar el código para cambiarlo usted mismo, especialmente en el bucle while para cambiar a FUERA DE BORDA, una vez que ocurre un accidente , debe matar este nodo para evitar que otros cambien el modo OFFBOARD, de lo contrario, su control remoto no puede apagar el OFFBOARD, solo el freno de mano