使用mavros啟動多機SITL仿真
- 方式1:使用roslaunch一鍵啟動
- Step1:創建一個新的 ROS 包或放到現有包里
- Step2:編輯 `multi_mavros.launch`
- Step3:構建工作空間并 source 環境
- Step4:構建工作空間并 source 環境
- 方式2:啟動單架無人機 MAVROS
為多個PX4 SITL無人機分別啟動各自的 mavros
實例進行通信。每架無人機都用一組不同的 UDP 和 TCP 端口進行連接。我們只需要為每架無人機配置一組唯一的 fcu_url
來啟動多個 mavros
節點。
方式1:使用roslaunch一鍵啟動
Step1:創建一個新的 ROS 包或放到現有包里
cd ~/catkin_ws/src
catkin_create_pkg my_multi_mavros
mkdir -p my_multi_mavros/launch
cd ~/catkin_ws/src/my_multi_mavros/launch
touch multi_mavros.launch
Step2:編輯 multi_mavros.launch
<launch><!-- UAV1 --><group ns="uav1"><include file="$(find mavros)/launch/px4.launch"><arg name="fcu_url" value="udp://:20101@127.0.0.1:20100"/><arg name="gcs_url" value=""/><arg name="tgt_system" value="1"/></include></group><!-- UAV2 --><group ns="uav2"><include file="$(find mavros)/launch/px4.launch"><arg name="fcu_url" value="udp://:20103@127.0.0.1:20102"/><arg name="gcs_url" value=""/><arg name="tgt_system" value="2"/></include></group><!-- UAV3 --><group ns="uav3"><include file="$(find mavros)/launch/px4.launch"><arg name="fcu_url" value="udp://:20105@127.0.0.1:20104"/><arg name="gcs_url" value=""/><arg name="tgt_system" value="3"/></include></group><!-- UAV4 --><group ns="uav4"><include file="$(find mavros)/launch/px4.launch"><arg name="fcu_url" value="udp://:20107@127.0.0.1:20106"/><arg name="gcs_url" value=""/><arg name="tgt_system" value="4"/></include></group>
</launch>
Step3:構建工作空間并 source 環境
cd ~/catkin_ws
catkin_make
source devel/setup.bash
Step4:構建工作空間并 source 環境
roslaunch my_multi_mavros multi_mavros.launch
方式2:啟動單架無人機 MAVROS
也可以單獨啟動 MAVROS 實例,比如:
roslaunch mavros px4.launch fcu_url:=udp://:20101@127.0.0.1:20100 \__ns:=uav1 tgt_system:=1roslaunch mavros px4.launch fcu_url:=udp://:20103@127.0.0.1:20102 \__ns:=uav2 tgt_system:=2roslaunch mavros px4.launch fcu_url:=udp://:20105@127.0.0.1:20104 \__ns:=uav3 tgt_system:=3roslaunch mavros px4.launch fcu_url:=udp://:20107@127.0.0.1:20106 \__ns:=uav4 tgt_system:=4
問題1: PX4 SITL 啟動時日志里寫的是 17541/16541
,那為什么 mavros
的 fcu_url
要用 20100/20101
而不是 17541/16541
?
20100
和 20101
是你自己在仿真系統(例如Gazebo )中 人為分配給 MAVROS 與 PX4 通信的中間橋接端口對,它們并不等于 PX4 SITL 啟動時用的 17541/16541
內部端口。
問題2: 為什么 uav1
用的是 20101/20100
,而 uav2
用的是 20103/20102
?為什么不是 20101/20100
和 20102/20101
?
啟動時已經為每架 MAVROS 與 PX4 的通信創建了一對端口:一個接收端(MAVROS本地端口)+一個發送端(PX4目標端口)。
端口增加“2”其實是人為設計的一種“互不沖突 + 成對編號”的方式 —— 每架無人機占用兩個連續端口,一個作為收,一個作為發。
? MAVROS 與 PX4 通信是雙向的:需要兩個端口
fcu_url:=udp://:20101@127.0.0.1:20100
含義是:
- MAVROS 接收數據:監聽
20101
(MAVROS本地UDP接收端) - MAVROS 發送數據:發到 PX4 的
127.0.0.1:20100
所以:
PX4
要配置為 將 MAVLink 數據發到20101
PX4
要監聽20100
來接收來自 MAVROS 的數據
這對端口從 MAVROS 視角是:
本地:20101 ← PX4:20100