1 编译AprilTag_ROS
AprilTag是一个视觉基准系统，可用于机器人，增强现实和相机校准等。 根据AprilTag可以可靠地计算标签相对于相机的3D位置，方向和ID号。这里我们使用AprilTag的ROS库来实现位姿估计与ID号计算。
编译命令如下：
sudo apt install ros-noetic-apriltag # 根据自己的ROS版本来修改
cp -r ~/XTDrone/sensing/object_detection_and_tracking/apriltag/apriltag_ros/ ~/catkin_ws/src/
cd ~/catkin_ws/
catkin build apriltag_ros #或catkin_make编译成功后的结果：会看到apriltag_ros包编译成功。
XTDrone团队已经做好了三个AprilTag并排的模型（如何制作见这里），在~/XTDrone/sitl_config/models/apriltag0-2，下面我们将让三个无人机分别降落到对应ID的AprilTag上。
2 配置PX4
在之前的任务中，我们没有让PX4 SITL发布过TF，一切坐标系转换都是依靠订阅话题+手工代码转换，由于在精准降落问题中，涉及到地图坐标系（map）、无人机坐标系（base_link）、相机坐标系（camera_link）和AprilTag坐标系（tag）四类坐标系，因此手工代码转换比较繁琐，因此我们借助ROS的TF工具来帮助我们进行转换。由于MAVROS默认不发布TF，且默认定义的坐标系不带namespace，也就是iris_0还是iris_1的机体坐标系都叫base_link，因此我们需要进行修改。
sudo cp ~/XTDrone/sitl_config/launch/px4/* /opt/ros/noetic/share/mavros/launch/3 修改EKF2设置
由于需要机体在Gazebo的地图坐标系下的位姿，因此定位需要使用Gazebo真值，此外使用Gazebo真值定位的精度也更高，便于事先精准降落。注意事先要修改EKF2设置，改为视觉定位模式。
所需修改部分的内容如下：
# GPS used
#param set EKF2_AID_MASK 1
# Vision used and GPS denied
param set EKF2_AID_MASK 24
# Barometer used for hight measurement
#param set EKF2_HGT_MODE 0
# Barometer denied and vision used for hight measurement
param set EKF2_HGT_MODE 34 启动仿真
4.1 启动三个带有下视相机的iris和带有AprilTag的无人车的仿真文件
roslaunch px4 outdoor2_precision_landing.launch 启动后会发现有报错：
或者如下的报错内容：
... logging to /home/ray/.ros/log/632bf21c-2b0a-11ef-ba9c-37ab60347b3f/roslaunch-ray-pc-391024.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
RLException: Invalid <param> tag: Cannot load command parameter [robot_description]: no such command [['/opt/ros/noetic/share/xacro/xacro.py', '/home/ray/catkin_ws/src/catvehicle/urdf/catvehicle1-3.xacro', 'roboname:=ugv_0']].
Param xml is <param name="robot_description" command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle1-3.xacro' roboname:='ugv_0'"/>
The traceback for the exception was written to the log file错因分析：问题主要出在xacro命令的路径和参数配置上，官方提供的<param>标签使用的是旧的命令格式，因此运行时会报错。
解决方法：去掉launch文件中<param>标签中xacro.py的.py
修改后的outdoor2_precision_landing.launch文件(该文件的目录：
/home/你的主机名/PX4_Firmware/launch)
修改后的完整代码：（可复制粘贴替换）
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor2.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<arg name="obstaclestopper" default="false"/>
<group ns="ugv_0">
<param name="robot_description"
command="$(find xacro)/xacro '$(find catvehicle)/urdf/catvehicle1-3.xacro' roboname:='ugv_0'" />
<include file="$(find catvehicle)/launch/catvehicle.launch">
<arg name="robot_name" value="ugv_0"/>
<arg name="init_pose" value="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0"/>
<arg name="config_file" value="catvehicle_control.yaml"/>
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
</include>
<!-- Uncomment this to get immediate motion from the car
<node name="openLoopCircle" pkg="safeopenloopcircle" type="safeopenloopcircle_node"/>
-->
</group>
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="-3"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_downward_camera"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
<arg name="config_yaml" value="$(find mavros)/launch/px4_config_iris_0.yaml"/>
</include>
</group>
<!-- iris_1 -->
<group ns="iris_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="-4.5"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_downward_camera"/>
<arg name="mavlink_udp_port" value="18571"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
<arg name="config_yaml" value="$(find mavros)/launch/px4_config_iris_1.yaml"/>
</include>
</group>
<!-- iris_2 -->
<group ns="iris_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="-6"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_downward_camera"/>
<arg name="mavlink_udp_port" value="18572"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
<arg name="config_yaml" value="$(find mavros)/launch/px4_config_iris_2.yaml"/>
</include>
</group>
<!-- <node pkg="tf" type="static_transform_publisher" name="0_base_link_to_base_link_frd"
args="0 0 -0.05 3.1415927 0 0 /base_link_0 /base_link_frd_0 100" />
<node pkg="tf" type="static_transform_publisher" name="1_base_link_to_base_link_frd"
args="0 0 -0.05 3.1415927 0 0 /base_link_1 /base_link_frd_1 100" />
<node pkg="tf" type="static_transform_publisher" name="2_base_link_to_base_link_frd"
args="0 0 -0.05 3.1415927 0 0 /base_link_2 /base_link_frd_2 100" /> -->
</launch>
若修改后还报错，输入如下命令检查是否安装xacro
which xacro
若已安装，则会有类似的输出
若没有，则输入如下命令安装xacro
sudo apt-get install ros-noetic-xacro
修改完成后再次启动仿真环境
roslaunch px4 outdoor2_precision_landing.launch 启动后的画面
正常启动后会发现不断报错，这是因为mavros的base_link_frd坐标系找不到base_link，因为我们给base_link加了namespace，不过这个错误不影响使用，不用理会。
[ERROR] [1617509823.832162747, 3.666000000]: ODOM: Ex: Could not find a connection between 'iris_0/base_link' and 'base_link_frd' because they are not part of the same tree.Tf has two or more unconnected trees.4.2 启动多机通信
4.2.1 修改multi_vehicle_communication.sh文件
注意事先修改好multi_vehicle_communication.sh，由于启动了3架iris无人机和1辆rover汽车，所以需要对multi_vehicle_communication.sh文件进行如下修改并保存。
4.2.2 输入如下命令启动多机通信
cd ~/XTDrone/communication
bash multi_vehicle_communication.sh通信启动成功后会有如下提示：
4.3 启动获取位资脚本
cd ~/XTDrone/sensing/pose_ground_truth/
python3 get_local_pose.py iris 34.4 启动键盘控制
cd ~/XTDrone/control/keyboard
python3 multirotor_keyboard_control.py iris 3 vel4.5 而后启动apriltag_ros
roslaunch apriltag_ros xtdrone_detection.launch4.6 用rviz进行可视化
cd ~/XTDrone/sensing/object_detection_and_tracking/apriltag/apriltag_ros/config
rviz -d rviz.rviz4.7 控制三架无人机到无人车的上方
观察rviz，会发现三个tag和一个tag bundle的坐标系在闪烁，这是因为这四个坐标系由三架飞机的apriltag_ros同时估计，因此只要有一架飞机能看到tag，就能估计其位姿。注意到图像中有飞机的影子，这个影子可能会干扰识别的效果，一个简单的方法是把gazebo中的灯光进行调整，不调整一般也能实现精准降落。
4.8 关闭键盘控制，启动多机精准降落脚本
4.8.1 在启动multi_precision_landing.sh脚本之前，先对原脚本进行修改，用python3来启动
4.8.2 启动多机精准降落脚本
cd ~/XTDrone/control/
bash multi_precision_landing.sh 降落效果展示：
说明：本次配置使用的是ubuntu20.04
参考： 多无人机精准降落 · 语雀
![[stm32]温湿度采集与OLED显示](https://img-blog.csdnimg.cn/direct/1fbb295a14724239a1cb80129cb99756.jpeg)
