[ROS] Conhecimento básico e uso do MAVROS

prefácio

O MAVLink é um protocolo de comunicação leve, usado principalmente para comunicação entre drones e estações terrestres, e contém muitas informações e comandos relacionados a drones, como status do drone, dados do sensor, energia da bateria, etc. Algumas plataformas de hardware UAV, como Pixhawk, PX4, ArduPilot, etc. usam comunicação MAVLink. O MAVROS é um pacote ROS de código aberto usado para conectar o protocolo ROS e MAVLink para realizar a comunicação e o controle entre o ROS e o UAV. Esta postagem é atualizada de tempos em tempos para registrar minha experiência de aprendizado do MAVROS.

1. instalação mavros

Mavros tem dois métodos de instalação: instalação de código-fonte e instalação binária.

(1) Instalação binária

# 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

Dois, sistema de coordenadas mavros

O sistema de coordenadas mavros é bastante confuso e é diferente do sistema de coordenadas comumente usado pela FCU (unidade de controle de voo, chamada de controle de voo) no PX4. O sistema de coordenadas usado pelo FCU do PX4 é o sistema de coordenadas NED (nordeste) ou FRD (frente inferior direito), mas o sistema de coordenadas comumente usado em mavros é ENU (céu nordeste) ou FLU (céu frontal esquerdo), mavros é convertido para mavlink ENU será convertido automaticamente para NED ao enviar para FCU. Claro, não precisamos nos preocupar com isso, só precisamos nos preocupar com qual sistema de coordenadas é necessário ao usar mavros.

(1) O sistema de coordenadas é dividido de acordo com a função

Sistemas de coordenadas comuns no tópico de mavros incluem sistema de coordenadas globais, sistema de coordenadas locais e sistema de coordenadas do corpo:

• O sistema global é fácil de entender, é um sistema de coordenadas GPS, como latitude e longitude, que não é muito usado;

• local refere-se ao sistema de coordenadas local, geralmente o sistema de coordenadas ENU, e a origem da coordenada geralmente está no ponto de inicialização do PX4;

• O sistema de coordenadas do corpo refere-se ao sistema de coordenadas do corpo. A versão cinética binária instalada do mavros tem um sistema de coordenadas do sistema de coordenadas RFU (frontal superior direito), e a origem das coordenadas está no corpo. As versões posteriores do mavros são alteradas uniformemente para o sistema de coordenadas FLU (canto superior esquerdo frontal);

(2) Divisão de orientação de instalação do sistema de coordenadas

• Sistema de coordenadas ENU: sistema de coordenadas do céu nordeste, também chamado de sistema de coordenadas do centro da estação, o sistema de coordenadas locais em mavros geralmente adota o sistema de coordenadas ENU, como tópico /mavros/setpoint_position/local;

• Sistema de coordenadas NED: Sistema de coordenadas Nordeste, também chamado de sistema de coordenadas de navegação, o sistema de coordenadas NED é usado no controle de vôo FCU do PX4;

• Sistema de coordenadas FLU: o sistema de coordenadas frontal superior esquerdo, ou seja, o sistema de coordenadas do corpo do UAV, com o UAV como origem, e é usado principalmente para o controle de velocidade do UAV. Por exemplo, a velocidade no tópico /mavros/setpoint_raw/localtópico é o sistema de coordenadas FLU;

Além disso, você pode modificar as configurações do sistema de coordenadas do tópico modificando os seguintes comandos:

roscd mavros
cd launch/
sudo gedit px4_config.yaml

O sistema de coordenadas no arquivo mapé globalo sistema e localo sistema, e base_linko sistema de coordenadas é bodyo sistema. Em seguida, o sistema de coordenadas correspondente a cada tópico específico de mavros precisa ser consultado em detalhes.

3. Tópicos comumente usados

（1）/mavros/estado

• Assinar/Publicar: Assinar

• Função: Assine alguns dados de status de mavros, como status de conexão, desbloqueio, modo drone atual

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

• Exemplo 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

• assinar/publicar:publicar

• Função: liberar o vôo apontador, o sistema de coordenadas atual é o sistema de coordenadas local local, ou seja, o sistema de coordenadas ENU (céu nordeste) com o FCU ligado como origem

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

• Exemplo 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 suplementar: Os sistemas de coordenadas comuns no tópico mavros incluem sistema global, sistema local e sistema corporal.

  O sistema global é fácil de entender, é um sistema de coordenadas GPS, como latitude e longitude, que não é muito usado.

  local refere-se ao sistema de coordenadas local, que se refere ao mapa local ou ao sistema de coordenadas mundial, geralmente o sistema de coordenadas ENU, e a origem das coordenadas geralmente é onde o PX4 é ligado.

  O sistema do corpo refere-se ao sistema de coordenadas do corpo, o que é confuso. Na versão cinética de mavros, o sistema do corpo é o sistema de coordenadas RFU (frente superior direito), e a origem das coordenadas está no corpo. No versão melódica e versões posteriores, o sistema do corpo foi alterado para o sistema de coordenadas FLU (canto superior esquerdo). (Frente aqui se refere à direção do nariz da aeronave)

（3）/mavros/local_position/pose

• Assinar/Publicar: Assinar

• Função: O conteúdo do tópico é a posição do sistema de coordenadas atual do UAV no sistema de coordenadas do mundo local. O sistema de coordenadas do mundo local é baseado no ponto de ativação do UAV PX4 como origem e a orientação de três eixos é Nordeste Céu (ENU); Coordenadas do UAV O sistema é o sistema de coordenadas do corpo, e a orientação dos três eixos é a frontal e superior esquerda; explique o tamanho do acelerômetro na IMU, ou seja, a aceleração linear. O acelerômetro considera o força resultante exceto a gravidade, ou seja, quando o drone está em queda livre, a aceleração do eixo Z é 0. Quando o drone é colocado no chão, a aceleração do eixo Z é +g, porque o drone é suportado pelo solo neste momento, a direção é verticalmente para cima e a magnitude é g, então não apenas porque Se a aceleração do eixo Z do drone colocado em repouso for +g, considera-se que o eixo Z direção é para baixo (pensando erroneamente que g é a gravidade)

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

• Exemplo 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

• Assinar/Publicar: Assinar

• Função: O conteúdo do tópico é a velocidade atual de três eixos do drone, incluindo a velocidade de três eixos e a velocidade angular de três eixos, e o sistema de coordenadas é o sistema de coordenadas local (com o ponto de ativação do drone como a origem e a direção nordeste)

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

• Exemplo 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/data(_raw)

• Assinar/Publicar: Assinar

• Função: O conteúdo do tópico são os dados de nove eixos da IMU (aceleração XYZ, velocidade angular XYZ, ângulo de atitude XYZ), data_raw são os dados originais e data são os dados filtrados (os dados do quaternion gerados pelo próprio px4 ) . O sistema de coordenadas IMU é o sistema de coordenadas frontal superior esquerdo do corpo.

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

• Exemplo 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

• assinar/publicar:publicar

• Função: Definir a aceleração do drone, mas o efeito é muito ruim, não é recomendado usar, se quiser controlar a aceleração, é recomendável usar o tópico de controle de empuxo

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

• assinar/publicar:publicar

• Função: Defina a velocidade linear e a velocidade angular do drone, e o sistema de coordenadas é o sistema de coordenadas local (nordeste do céu)

• Tipo de dados: geometry_msgs/Twist

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

• Exemplo 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/atitude

• assinar/publicar:publicar

• Função: Definir atitude UAV, velocidade angular e impulso

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

• Exemplo 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. Serviços comumente usados

（1）/mavros/cmd/arming

• Servidor/Cliente: Cliente

• Função: emitir comando de desbloqueio/bloqueio

• Tipo de dados: mavros_msgs/CommandBool

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

• Exemplo 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

• Função: Solicitação para alternar o modo de voo da FCU

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

• Exemplo 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: Tenha cuidado ao mudar para o modo OFFBOARD, especialmente em vôo real, é melhor usar o controle remoto para mudar para OFFBOARD em vez de usar o código para mudar você mesmo, especialmente no loop while para mudar para OFFBOARD, uma vez que ocorra um acidente , você precisa matar este nó para evitar outros Mude o modo OFFBOARD, caso contrário, seu controle remoto não pode desligar o OFFBOARD, apenas o freio de mão