【ROS】MAVROSの基礎知識と使い方

序文

MAVLink は軽量の通信プロトコルで、主にドローンと地上局間の通信に使用され、ドローンのステータス、センサー データ、バッテリー電力など、ドローンに関連する多くの情報とコマンドが含まれています。Pixhawk、PX4、ArduPilot などの一部の UAV ハードウェア プラットフォームは MAVLink 通信を使用します。MAVROS は、ROS と MAVLink プロトコルを接続し、ROS と UAV 間の通信と制御を実現するために使用されるオープンソース ROS パッケージです。この投稿は、MAVROS の学習体験を記録するために随時更新されます。

1.マブロスのインストール

Mavros には、ソース コード インストールとバイナリ インストールという 2 つのインストール方法があります。

(1) バイナリのインストール

# 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

2、マブロス座標系

マブロス座標系は非常にややこしく、PX4のFCU（フライトコントロールユニット、いわゆる飛行制御）で一般的に使用される座標系とは異なります。PX4のFCUで使用される座標系はNED(北東)またはFRD(前方右下)座標系ですが、マブロスで一般的に使用される座標系はENU(北東の空)またはFLU(前方左の空)であり、マブロスは変換されます。 to mavlink ENU は、FCU に送信するときに自動的に NED に変換されます。もちろん、これらを気にする必要はありません。mavros を使用するときにどの座標系が必要かだけを気にする必要があります。

(1) 座標系を機能別に分ける

マブロスのトピックにおける一般的な座標系には、グローバル座標系、ローカル座標系、およびボディ座標系が含まれます。

• 地球座標系は分かりやすく、あまり使われない緯度経度などのGPS座標系です。

• local はローカル座標系 (通常は ENU 座標系) を指し、座標原点は通常 PX4 の電源投入点にあります。

• ボディ座標系とはボディの座標系のことで、バイナリインストールされたキネティック版のmavrosはRFU(右上)座標系の座標系を持ち、座標の原点はボディにあります。mavros の以降のバージョンは、一律に FLU (前面左上) 座標系に変更されます。

(2) 座標系設置方向部門

• ENU 座標系: 北東の空の座標系。駅中心座標系とも呼ばれます。マブロスのローカル座標系は、一般にトピックなどの ENU 座標系を採用します/mavros/setpoint_position/local。

• NED 座標系: 北東座標系。ナビゲーション座標系とも呼ばれます。NED 座標系は PX4 の飛行制御 FCU で使用されます。

• FLU 座標系: 正面左上の座標系、つまり UAV を原点とする UAV 本体座標系は、主に UAV の速度制御に使用されます。たとえば、トピックの速度は FLU です/mavros/setpoint_raw/local。座標系。

さらに、次のコマンドを変更することで、トピックの座標系設定を変更できます。

roscd mavros
cd launch/
sudo gedit px4_config.yaml

ファイル内の座標系はシステムとシステムmapであり、座標系はシステムです。次に、マブロスのそれぞれの特定のトピックに対応する座標系を詳細にクエリする必要があります。globallocalbase_linkbody

3. よく使用されるトピック

（1）/マブロス/州

• 購読/公開: 購読する

• 機能: 接続ステータス、ロックを解除するかどうか、現在のドローンモードなど、マブロのいくつかのステータスデータをサブスクライブします。

• データ型: 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/setpoint_position/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);
  

• 補足：マヴロスの話題における共通座標系には、グローバル系、ローカル系、ボディ系が含まれます。

  地球座標系は分かりやすく、あまり使われることのない緯度経度などのGPS座標系です。

  local はローカル座標系を指します。これはローカル マップまたはワールド座標系 (通常は ENU 座標系) を指し、座標原点は通常、PX4 の電源がオンになっている場所です。

  ボディシステムとはボディ上の座標系のことを指しますが、これは紛らわしいですが、キネティックバージョンのマブロスの場合、ボディシステムは RFU (右上) 座標系であり、座標の原点はボディにあります。メロディック版以降はボディ系がFLU(正面左上)座標系に変更されました。（ここでいう前とは航空機の機首の方向を指します）

（3）/mavros/local_position/pose

• 購読/公開: 購読する

• 機能: トピックの内容は、ローカル ワールド座標系における現在の UAV 座標系のポーズです。ローカル ワールド座標系は、UAV PX4 の電源投入点を原点とし、3 軸方向は北東です。天 (ENU); UAV 座標系は本体座標系であり、3 軸の向きは前方と左上です; IMU の加速度計のサイズ、つまり線形加速度を説明します。重力を除いた合力、つまりドローンが自由落下しているとき、Z 軸の加速度は 0 です。ドローンが地面に置かれているとき、Z 軸の加速度は +g です。このとき地面に支えられている方向は鉛直上向き、大きさはgなので、単に静止しているドローンのZ軸加速度が+gであれば、Z軸は加速度が高いと考えられます。方向は下（gを重力と勘違い）

• データ型: 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);
  
  

（4）/mavros/local_position/velocity_local

• 購読/公開: 購読する

• 機能: トピックの内容は、3 軸速度と 3 軸角速度を含むドローンの現在の 3 軸速度であり、座標系はローカル座標系 (ドローンの電源投入点を使用)原点と北東方向）

• データ型: 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;
  

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

• 購読/公開: 購読する

• 関数: トピックの内容は IMU 9 軸データ (XYZ 加速度、XYZ 角速度、XYZ 姿勢角)、data_raw は元のデータ、data はフィルター処理されたデータ (px4 自身が生成した四元数データ)です。IMU 座標系は、前部左上体の座標系です。

• データ型: 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;
  

（6）/mavros/setpoint_accel/accel

• サブスクライブ/パブリッシュ:パブリッシュ

• 機能: ドローンの加速度を設定しますが、効果は非常に低いため、使用はお勧めしません。加速度を制御したい場合は、推力制御のトピックを使用することをお勧めします。

• データ型: 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

• サブスクライブ/パブリッシュ:パブリッシュ

• 機能：ドローンの線速度と角速度を設定、座標系はローカル座標系（北東の空）

• データ型: 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();
  

（8）/mavros/setpoint_raw/attitude

• サブスクライブ/パブリッシュ:パブリッシュ

• 機能: UAV の姿勢、角速度、推力を設定します。

• データ型: 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. よく利用されるサービス

（1）/mavros/cmd/arming

• サーバー/クライアント: クライアント

• 機能: ロック解除/ロックコマンドを発行します。

• データ型: 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

• サーバー/クライアント: クライアント

• 機能：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");
  }
  

• 注: オフボード モードへの切り替えには注意してください。特に実際の飛行では、コードを使用して自分で切り替えるのではなく、リモコンを使用してオフボード モードに切り替えることをお勧めします。特に、事故が発生した場合は、オフボードに切り替える while ループ内で、他のノードを防ぐために、このノードを強制終了する必要があります。 オフボード モードに切り替えてください。そうしないと、リモコンでオフボードを切り替えることができず、ハンドブレーキのみがオフになります。