介绍

为https://blog.csdn.net/weixin_41469272/article/details/117919845的补充篇

主要介绍px4 的软硬件在环仿真，可以实现仿真世界的slam定位，以及仿真世界的遥控飞行(HITL）。
mavros使用的坐标系是ENU，与gazebo相同，可以通过gazebo世界中看坐标系(红绿蓝分别指xyz)观察无人机的世界坐标系。默认无人机机头朝向x方向，则无人机惯性体坐标系通常为(前左上)。

依赖

• Dependencies：
  PX4 Firmware v1.8.0
  地面站：QGC
  视景：gazabo
  通信：mavros

PX4 Firmware：

下载：

git clone https://github.com/PX4/PX4-Autopilot.git --recursive

编译固件：

cd [PX4-Autopilot]
# 编译v1.8.0
#git checkout v1.8.0
#编译对应硬件版本的v1.8。0的固件，以下以v5为例，vx与飞控硬件版本号对应
make px4_fmu-v5_default
#编译sitl仿真程序
DONT_RUN=1 make px4_sitl_default gazebo-classic
或
make px4_sitl gazebo 

烧录v1.8.0固件
1）USB连接飞控ACM口
2）打开QGC，设置->固件
3）重新拔插USB-》弹出固件设置->勾选高级设置-》下拉框选择自定义固件-》弹出文件管理框-》找到编译好的固件px4_fmu-vx_default.px4
固件路径默认在[PX4-Autopilot]/build/nuttx_px4fmu-vx_default下
sitl程序默认在[PX4-Autopilot]/build/posix_sitl_default下

软件在环(SITL)仿真

Gazebo 软件无人机

1. 更新环境变量

source [PX4-Autopilot]/Tools/setup_gazebo.bash [PX4-Autopilot] [PX4-Autopilot]/build/posix_sitl_default

2. launch mavros sitl
   （1）仅启动STIL

cd [PX4-Autopilot]/launch
#仅启动gazebo和仿真无人机
roslaunch posix_sitl.launch

附posix_sitl.launch文件内容

<?xml version="1.0"?>
<launch>
    <!-- Posix SITL environment launch script -->
    <!-- launches PX4 SITL, Gazebo environment, and spawns vehicle -->
    <!-- vehicle pose -->
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- PX4 SITL -->
    <arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
    <arg     if="$(arg interactive)" name="px4_command_arg1" value=""/>
    <node name="sitl" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS) $(arg px4_command_arg1)" required="true"/>
    <!-- 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)"/>
        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
    </include>
    <!-- gazebo model -->
    <node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf -file $(arg sdf) -model $(arg vehicle) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
</launch>
~          

此时在终端回车，可以使用pxh命令，即px4的终端指令。
如commander takeoff让飞机起飞
以及一些常用的mavlink命令，如启用飞控硬件串口，修改串口波特率等等。
eg；

mavlink start -d /dev/ttyS2 -b 1000000 -r 1000 -m onboard
mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 1000
mavlink stream -d /dev/ttyS2 -s VISION_POSE_ESTIMATE -r 400 
mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 1000

更多指令见本博客后续的介绍

（2）启动STIL，及mavros 地面站

cd [PX4-Autopilot]/launch
#启动gazebo和仿真无人机，以及mavros
roslaunch mavros_posix_sitl.launch

附mavros_posix_sitl.launch

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
    <!-- vehicle pose -->
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
    <!-- MAVROS configs -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="respawn_mavros" default="false"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- PX4 SITL and Gazebo -->
    <include file="$(find px4)/launch/posix_sitl.launch">
        <arg name="x" value="$(arg x)"/>
        <arg name="y" value="$(arg y)"/>
        <arg name="z" value="$(arg z)"/>
        <arg name="R" value="$(arg R)"/>
        <arg name="P" value="$(arg P)"/>
        <arg name="Y" value="$(arg Y)"/>
        <arg name="world" value="$(arg world)"/>
        <arg name="vehicle" value="$(arg vehicle)"/>
        <arg name="sdf" value="$(arg sdf)"/>
        <arg name="rcS" value="$(arg rcS)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="interactive" value="$(arg interactive)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
    </include>
    <!-- MAVROS -->
    <include file="$(find mavros)/launch/px4.launch">
        <!-- GCS link is provided by SITL -->
        <arg name="gcs_url" value=""/>
        <arg name="fcu_url" value="$(arg fcu_url)"/>
        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
    </include>
</launch>

3. 连接地面站

打开地面站，(确保地面站设置->常规->自动连接到下列设备->UDP被勾选)，则会自动连接到模型飞机

而后使用QGC可以对飞机进行一些操作。

4. 其他

此外，在单独launch了posix_sitl.launch，也可以通过下述命令单独启动mavros，连接仿真无人机及地面站:

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

默认自动开启gcs_url，也通过如下命令，设置远程地面站(要求地面站与仿真电脑同一局域网)

roslaunch mavros px4.launch fcu_url:=udp://:14540@127.0.0.1:14557 gcs_url:=udp://@192.168.xxx.xxx

通过以下命令可以查看mavros的连接状态

rostopic echo /mavros/state

STIL连接简要示意

GCS《-可远程连接(端口:IP)-》mavros:udp:14540 《-》px4:udp:14557 的连接，以下是连接示意图。
也可以使用QGC连接游戏摇杆，控制仿真无人机。或者带有仿真遥控功能的地面站。
https://doc.cuav.net/tutorial/px4-tutorial/zh-hans/config/joystick.html

可以使用netstat -tunlp |grep px4 或netstat -tunlp |grep 14557来查看端口占用状态。


mavros指定14540<arg name="fcu_url" default="udp://:14540@localhost:14557"/>

SITL SLAM仿真

本实验是基于D435i仿真模型是按的RGBD SLAM仿真。

1. 模型创建

建立安装D435i的无人机仿真模型。详细参考：https://blog.csdn.net/weixin_41469272/article/details/117919845
模型下载地址: https://gitee.com/nie_xun/realsense_ros_gazebo.git

根据上述链接，我们可以得到D435i，无人机，以及搭建D435i的无人机的模型，将其模型对应的文件夹放到[PX4-Autopilot]/Tools/sitl_gazebo/models下。

注意v1.8.0的UAV模型通过rcS(默认路径/posix-configs/SITL/init/$(arg est)/$(arg vehicle))下修改位置源(EKF2_AID_MASK)。附示例rcS文件。

uorb start
param load
dataman start
param set BAT_N_CELLS 3
param set CAL_ACC0_ID 1376264
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_ID 1310728
param set CAL_ACC1_XOFF 0.01
param set CAL_GYRO0_ID 2293768
param set CAL_GYRO0_XOFF 0.01
param set CAL_MAG0_ID 196616
param set CAL_MAG0_XOFF 0.01
param set COM_DISARM_LAND 3
param set COM_OBL_ACT 2
param set COM_OBL_RC_ACT 0
param set COM_OF_LOSS_T 5
param set COM_RC_IN_MODE 1
param set EKF2_AID_MASK 24
param set EKF2_ANGERR_INIT 0.01
param set EKF2_GBIAS_INIT 0.01
param set EKF2_HGT_MODE 3
param set EKF2_MAG_TYPE 1
param set MAV_TYPE 2
param set MC_PITCH_P 6
param set MC_PITCHRATE_P 0.2 
param set MC_ROLL_P 6
param set MC_ROLLRATE_P 0.2 
param set MIS_TAKEOFF_ALT 2.5 
param set MPC_HOLD_MAX_Z 2.0 
param set MPC_Z_VEL_I 0.15
param set MPC_Z_VEL_P 0.6 
param set NAV_ACC_RAD 2.0 
param set NAV_DLL_ACT 2
param set RTL_DESCEND_ALT 5.0 
param set RTL_LAND_DELAY 5
param set RTL_RETURN_ALT 30.0
param set SDLOG_DIRS_MAX 7
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set SYS_AUTOSTART 4010
param set SYS_MC_EST_GROUP 2
param set SYS_RESTART_TYPE 2
replay tryapplyparams
simulator start -s
tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start
pwm_out_sim start
sensors start
commander start
land_detector start multicopter
navigator start
ekf2 start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -x -u 14556 -r 4000000
mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 50 -s ATTITUDE -u 14556
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
logger start -e -t
mavlink boot_complete
replay trystart

将编译得到的librealsense_gazebo_plugin.so存放到[PX4-Autopilot]//build/posix_sitl_default/build_gazebo下或者export到LD_LIBRARY_PATH下。

2. px4 mavros gazebo launch
   创建launch文件，同时启动gazebo models mavros以及PX4，以下为示例launch文件
   iris_realsense_camera_px4_mavros_vo.launch

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
    <!-- vehicle pose -->
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <!--arg name="world" default="$(find gazebo_ros)/worlds/[your world].world"/-->
    <arg name="world" default="$(find px4)/Tools/sitl_gazebo/worlds/vio_simple4.world"/>
    <!--arg name="sdf" default="$(find px4)/Tools/sitl_gazebo/models/[your model]/[your model].sdf"/-->
    <arg name="sdf" default="$(find px4)/Tools/sitl_gazebo/models/iris_realsense_camera/iris_realsense_camera.sdf"/>
    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_vo"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
    <!-- MAVROS configs -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="respawn_mavros" default="false"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- PX4 SITL and Gazebo -->
    <include file="$(find px4)/launch/posix_sitl.launch">
        <arg name="x" value="$(arg x)"/>
        <arg name="y" value="$(arg y)"/>
        <arg name="z" value="$(arg z)"/>
        <arg name="R" value="$(arg R)"/>
        <arg name="P" value="$(arg P)"/>
        <arg name="Y" value="$(arg Y)"/>
        <arg name="world" value="$(arg world)"/>
        <arg name="vehicle" value="$(arg vehicle)"/>
        <arg name="sdf" value="$(arg sdf)"/>
        <arg name="rcS" value="$(arg rcS)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="interactive" value="$(arg interactive)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
    </include>
    <!-- MAVROS -->
    <include file="$(find mavros)/launch/px4.launch">
        <arg name="gcs_url" value=""/>
        <arg name="fcu_url" value="$(arg fcu_url)"/>
        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
    </include>
</launch>

其中的posix_sitl.launch(可见上文)以及px4.launch可以用px4自带文件。

3. slam launch
   重点remap gazebo模型输出的相机topic到slam，此外remap SLAM输出的定位信息remap到/mavros/vision_pose/pose，从而，mavros自动解析local position到飞控。

以下为launch topic remap 示例：

1. IMU topic
<!--remap from="/your slam/imu topic" to="/camera/imu" /-->
eg:
<remap from="/imu/data_raw" to="/camera/imu" />
2. rgb image topic
<!--remap from="/your image topic" to="/camera/color/image_raw" /-->
eg:
<remap from="/camera/rgb/image_rect" to="/camera/color/image_raw" />
3. depth image topic
<!--remap from="/your depth image topic" to="/camera/depth/image_raw" /-->
eg:
<remap from="/camera/depth_registered/image" to="/camera/depth/image_raw" />

4. odometry topic
<remap from="your odomtry topic" to="/mavros/vision_pose/pose" />

4. Offboard launch
   编写任务机目标程序。

方形目标文件：offboard_square.cpp

#include <string>
#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>
#include <vector>

using namespace std;

ros::Subscriber state_sub, local_pos_sub;
ros::Publisher pose_target_pub;

mavros_msgs::State current_state;
int pos_state = 0;
std::vector<geometry_msgs::PoseStamped> pose_commands;

vector<double> initPose;
float takeoff_height;
float side_length;

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    if( current_state.mode != "OFFBOARD" )
    return;
    geometry_msgs::Point p_now = msg->pose.position;
    geometry_msgs::Point p_target = pose_commands[pos_state].pose.position;

    double delta_p = sqrt((p_target.x + initPose[0] - p_now.x)*(p_target.x + initPose[0] - p_now.x) + (p_target.y + initPose[1] - p_now.y)*(p_target.y + initPose[1] - p_now.y) + 
    (p_target.z + initPose[2] - p_now.z)*(p_target.z + initPose[2] - p_now.z));

    std::cout << "delta_p:" <<  delta_p << std::endl;

    if(delta_p < 0.3 && pos_state + 1 < pose_commands.size()){
        pos_state++;
        std::cout << "pos_state: " << pos_state << std::endl;
    }   
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh; 

    if(!nh.getParam("initPose", initPose)) {
      ROS_ERROR_STREAM("Read param error initPose:");
      for (int i = 0; i < initPose.size(); i++) {
        std::cout << "initPose: " << i << ": " << initPose[i] << std::endl;
      }   
    }
    nh.param<float>("takeoff_height", takeoff_height, 1);
    nh.param<float>("side_length", side_length, 1);

    state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose", 10, local_pos_cb);
    pose_target_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = takeoff_height;
    pose.pose.orientation.w = 1;
    pose.pose.orientation.x = 0;
    pose.pose.orientation.y = 0;
    pose.pose.orientation.z = 0;

    pose_commands.push_back(pose);
    pose.pose.position.y = -side_length;
    pose_commands.push_back(pose);
    pose.pose.position.x = side_length;
    pose_commands.push_back(pose);
    pose.pose.position.y = 0;
    pose_commands.push_back(pose);
    pose.pose.position.x = 0;
    pose_commands.push_back(pose);
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        pose_target_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    int count = 0;
    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" && count%20 == 0){
            //local_pos_pub.publish(pose);
            std::cout << current_state.mode << std::endl;
        }
        pose_target_pub.publish(pose_commands[pos_state]);
        ros::spinOnce();
        rate.sleep();
    count++;
    }

    return 0;
}

方形节点启动launch Offboard_square.launch

<?xml version='1.0'?>
<launch>
  <rosparam>
    initPose: [0, 0, 0]
    side_length: 2
    takeoff_height: 2
  </rosparam>
  <node pkg="offboard" type="offboard_square_node" name="offboard_square_node" output="screen">
  </node>
</launch>

5. 坐标系变换
   当SLAM坐标系与mavros使用的坐标系(ENU，FLU)不相同时，在remap 定位消息之前，需要先进行坐标系对齐。
   两种情况，当两个坐标系是同手系坐标系(同为左手坐标系或右手坐标系)，则可以手动对齐yaw角(旋转相机安装方式)，就可以直接remap消息。
   当两个坐标系，一个是左手坐标系一个是右手坐标系时，则需要进行坐标变换。https://blog.csdn.net/weixin_41469272/article/details/117919845 nav_msg_to_mavros.cpp举了一个只需要变换坐标轴定义的例子。

启动offboard方形以及转换坐标系，及remap topic的例子Offboard_square_vision_pose_simulation.launch

<?xml version='1.0'?>
<launch>
  <rosparam>
    initPose: [0, 0, 0]
    side_length: 2
    takeoff_height: 2
  </rosparam>
  <node pkg="offboard" type="offboard_square_node" name="offboard_square_node" output="screen"/>
  <param name="use_sim_time" type="bool" value="True"/>
  <node pkg="offboard" type="offboard_nav_msg_to_mavros_node" name="offboard_nav_msg_to_mavros_node" output="screen">
    <!-- 0: gazebo_gt(NWU); 1: camera_vo(WUN)-->
    <param name="data_source" value="1" />
    <remap from="/camera/odometry" to="/cam_to_init" />
    <!--remap from="/camera/odometry" to="/ground_truth/iris" /-->
  </node>
</launch>

总结示例

3个窗口一个启动一个，自用

1. px4 mavros eg
#cd in ~/workspace/uav_ws/src/Firmware/launch
#source [PX4-Autopilot]/Tools/setup_gazebo.bash [PX4-Autopilot] [PX4-Autopilot]/build/posix_sitl_default

roscd px4/launch
#launch uav & world
roslaunch iris_realsense_camera_px4_mavros_vo.launch

2. slam eg
cd ~/workspace/demo_tmp/src/demo_rgbd/launch
#launch slam
#source ~/workspace/demo_tmp/devel/setup.sh
roslauch roslaunch realsense_simulation.launch

3. offboard eg
cd ~/workspace/uav_ws/src/Offboard_simulation/src/offboard/launch
#launch offboard localization & mission
#source ~/workspace/uav_ws/src/Offboard_simulation/devel/setup.sh
roslaunch Offboard_square_vision_pose_simulation.launch

硬件在环(HITL) SLAM 仿真

与SITL不同的是，硬件在环的飞控及offboard任务机连接实际硬件。地面站可远程或直接部署在任务机上(因为是仿真，世纪飞行，地面站需要远程)。
连接示意图如下：

推荐：如果只是想测试slam，而不是为飞行作准备，可以将任务机、视景计算机、地面站放在一个电脑，如官方连接所示：

此外，如果无人机的定位源不用SLAM输出，则mavros也不用起，gazebo加载模型文件后，地面站便可以通过指定的端口号进行连接（GCS<端口号14550>gazebo<硬件串口>sdf），无人机模型文件可以指定gcs监听的端口(v1.8.0: 14550)，如下所示：

  <qgc_addr>INADDR_ANY</qgc_addr>
  <qgc_udp_port>14550</qgc_udp_port>

当需要将SLAM替换到飞控的定位源，也可以使用两路硬件，一路飞控硬件连接到视景计算机（用于连接模型），一路视景计算机作为任务机启动mavros连接（用于地面站以及发送offboard程序）。如下iris_realsense_camera_mavros_hil.launch所示：

 36     <arg name="fcu_url" default="/dev/ttyUSB0:1000000"/>

1. 飞控设置
   QGC通过ACM链接飞控（注意，QGC常规设置->自动连接到下列设备pixhawk需要选中）
   

设置机架, 选择HIL Quadcopter X，此时会提示"HITL被启用"一类的警告。同时设置->安全->最后一项硬件在环仿>
真自动被启用"HITL enabled"。

如果不连接遥控器RC，则需要设置以下参数

• COM_RC_IN_MODE to “Joystick/No RC Checks”. 这允许操纵杆输入并禁用 RC 输入检查。
• NAV_RCL_ACT to “Disabled”. 这可确保在没有无线遥控的情况下运行 HITL 时 RC 失控保护不会介入。
  想用遥控器控制，则把接收器查到飞控硬件上。

2. 硬件在环模型文件
   与SITL模型文件不同的是，HITL需要修改无人机模型文件：如[PX4-Autopilot]/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_hitl/iris_hitl.sdf
   将模型文件对应的 mavlink_interface plugin部分的 serialEnabled 和 hil_mode改为 true。
   并对应修改模型的串口serialDevice为使用的连接端口。可以通过拔插飞控，并使用dmesg | grep "tty"或ls /dev/tty*查看多出来的设备号。
   hil_state_level设置为0。在 PX4 HIL 仿真中，可以选择多种 HIL 模式，比如传感器级别（Sensors HIL）和姿态级别（Attitude HIL）。hil_state_level 就是用来配置这个模式。它通常会被设置成 0 或 1。

hil_state_level参数介绍：
0：表示传感器级别的 HIL 模式，Gazebo 将传感器数据（如加速度计、陀螺仪、GPS）直接发送到 PX4，PX4 将按照这些仿真传感器数据进行控制。
1：表示姿态级别的 HIL 模式，Gazebo 会发送飞机的姿态信息（如 roll、pitch、yaw 角度）而不是详细的传感器数据。
选择的影响：
传感器级别：更精细化的控制，PX4 可以根据传感器数据运行其内部传感器融合算法，适合测试姿态控制器的表现。
姿态级别：简化数据传输量，可以减少计算负担，但缺少细粒度的传感器模拟，适合快速测试高层控制逻辑。

<plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace/>
      <imuSubTopic>/imu</imuSubTopic>
      <gpsSubTopic>/gps</gpsSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      **<serialEnabled>true</serialEnabled>**
      **<serialDevice>/dev/ttyACM0</serialDevice>**
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      **<hil_mode>true</hil_mode>**
      **<hil_state_level>0</hil_state_level>**
      **<update_rate>500</update_rate>**---可选

sdf中的libgazebo_xxx_plugin.so xxx 可以是gps，imu等，表现gazebo会在仿真世界中模拟对应的传感器数据。
libgazebo_mavlink_interface.so通常用于连接 Gazebo 与 PX4 或其他飞控系统。通过这个接口，可以配置和控制飞行器与模拟环境之间的通信，包括传感器数据的交换（例如 GPS 数据、IMU 数据等）。即，使用这个interface，gazebo会将对应的仿真传感器数据生成mavlink消息，通过指定的串口传输给px4。

serialEnabled：是否启用串口通信，1 表示启用，0 表示禁用。启用时，插件会通过串口将 MAVLink 消息发送给飞控。
serialDevice：指定串口设备路径，例如 /dev/ttyACM0。
baudRate：串口波特率，通常与飞控的波特率相匹配，常见的波特率包括 57600、115200 等。
mavlink_udp_port：指定 MAVLink 使用的 UDP 端口，例如 14550。飞控或地面站可以通过这个端口接收 MAVLink 消息。
gpsSubTopic、baroSubTopic、imuSubTopic：指定用于发布 GPS、气压计和 IMU 数据的 Gazebo 主题（topic）。这些数据会被插件采集并转发给飞控。
hil_state_level：设置 HIL（硬件在环）模式的级别。1 表示将飞行状态（如高度、姿态等）发送给飞控。
此外，可通过update_rate 参数控制了消息的发送频率。增大这个参数可以提高消息传输速率。

说明：libgazebo_mavlink_interface.so plugin尽量放到sdf文件的最后，即放于所有传感器仿真plugin之后，如果传感器plugin放于之前，再启动mavlink plugin时可能会出现传感器数据不被发布的问题。
此外，直接将sdf模型文件include到world文件中，使用gazebo_ros去include world发生传感器数据不被传输的概率较小，是推荐的启动方式。
该启动launch，world等文件已示例放于https://gitee.com/nie_xun/realsense_ros_gazebo.git

3. 硬件在环launch
   使用gazebo打开仿真世界以及(mavros）
   示例 iris_realsense_camera_mavros_hil.launch

  1 <?xml version='1.0'?>
  2 <launch>
  3     <!-- Gazebo configs -->
  4     <arg name="gui" default="true"/>
  5     <arg name="debug" default="false"/>
  6     <arg name="verbose" default="false"/>
  7     <arg name="paused" default="false"/>
  8     <arg name="use_sim_time" default="false"/>
  9     <!--arg name="headless" default="false"/-->
 10     <arg name="world" default="$(find px4)/Tools/sitl_gazebo/worlds/vio_simple4.world"/>
 11     <!-- Gazebo sim -->
 12     <include file="$(find gazebo_ros)/launch/empty_world.launch">
 13         <arg name="gui" value="$(arg gui)"/>
 14         <arg name="debug" value="$(arg debug)"/>
 15         <arg name="verbose" value="$(arg verbose)"/>
 16         <arg name="paused" value="$(arg paused)"/>
 17         <arg name="use_sim_time" value="$(arg use_sim_time)"/>
 18         <!--arg name="headless" value="$(arg headless)"/-->
 19         <arg name="world_name" value="$(arg world)"/>
 20     </include>
 21 
 22     <!-- Spawn vehicle model -->
 23     <arg name="x" default="0.0"/>
 24     <arg name="y" default="0.0"/>
 25     <arg name="z" default="0.0"/>
 26     <arg name="R" default="0"/>
 27     <arg name="P" default="0"/>
 28     <arg name="Y" default="0.0"/>
 29     <arg name="sdf" default="$(find px4)/Tools/sitl_gazebo/models/iris_realsense_camera_hil/iris_realsense_camera_hil.sdf"/>
 30     <arg name="model" default="iris_realsense_camera_hil"/>
 31     <node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" 
 32         args="-sdf -file $(arg sdf) -model $(arg model) -x $(arg x) -y $(arg y) -z $(arg z) -R $(    arg R) -P $(arg P) -Y $(arg Y)">
            <param name="wait_for_gazebo" value="true" />
 33     </node>
 34     
 35     <!--arg name="fcu_url" default="udp://:14540@localhost:14557"/-->
 36     <arg name="fcu_url" default="/dev/ttyUSB0:1000000"/>
 37     <!-- 启动MAVROS --> 
 38     <!--node pkg="mavros" type="mavros_node" name="mavros" output="screen">
 39         <param name="fcu_url" value="$(arg fcu_url)"/>
 40         <param name="target_system_id" value="1" />
 41         <param name="target_component_id" value="1" />
 42         <rosparam command="load" file="$(find px4)/config/mavros_config/px4_pluginlists.yaml" />
 43         <rosparam command="load" file="$(find px4)/config/mavros_config/px4_config.yaml" />
 44     </node-->
 45     
 46 </launch>

4. 任务机部署
   1）将offboard 方形轨迹、坐标变换、mavros部署在任务机上。

示例：Offboard_square_vision_pose.launch

<?xml version='1.0'?>
<launch>
  <node pkg="mavros" type="mavros_node" name="mavros" output="screen">
    <param name="fcu_url" value="/dev/ttyUSB0:921600" />
    <!--param name="gcs_url" value="udp://@192.168.xxx.xxx<-your GCS IP " /-->
    <param name="gcs_url" value="" />

    <param name="target_system_id" value="1" />
    <param name="target_component_id" value="1" />
    <rosparam command="load" file="$(find offboard)/config/mavros_config/px4_pluginlists.yaml" />
    <rosparam command="load" file="$(find offboard)/config/mavros_config/px4_config.yaml" />
  </node>

  <rosparam>
    initPose: [0, 0, 0]
    side_length: 2
    takeoff_height: 2
  </rosparam>
  <node pkg="offboard" type="offboard_square_node" name="offboard_square_node" output="screen"/>

  <node pkg="offboard" type="offboard_nav_msg_to_mavros_node" name="offboard_nav_msg_to_mavros_node" output="screen">
    <!-- 0: gazebo_gt(NWU); 1: camera_vo(WUN)-->
    <param name="data_source" value="1" />
    <remap from="/camera/odometry" to="/cam2_to_init" />
    <!--remap from="/camera/odometry" to="/ground_truth/iris" /-->
  </node>

</launch>

注意：与SITL 不同的是如果任务机与视景软件分离，任务机上启动mavros，视景软件上不要起mavros了，即iris_realsense_camera_mavros_vo_hil.launch去掉mavros node。

如果任务机与视景集成，且需要替换定位源，则需要启动mavros，用iris_realsense_camera_mavros_vo_hil.launch和Offboard_square_vision_pose_simulation.launch去掉mavros节点启动。

2） gazebo模型位姿真值记录为TUM
源文件示例odom_recorder.cpp

 1 #include <ros/ros.h>
  2 #include <nav_msgs/Odometry.h>
  3 #include <fstream>
  4 #include <iostream>
  5 #include <sstream>
  6 #include <iomanip>
  7 
  8 class OdomRecorder {
  9 public:
 10     OdomRecorder(ros::NodeHandle& nh, const std::string& filename) : out_file_(filename, std::ios    ::out) {
 11         if (!out_file_.is_open()) {
 12             ROS_ERROR("Failed to open file: %s", filename.c_str());
 13             ros::shutdown();
 14         }
 15         sub_ = nh.subscribe("/ground_truth/iris", 10, &OdomRecorder::odomCallback, this);
 16     }
 17 
 18     ~OdomRecorder() {
 19         if (out_file_.is_open()) {
 20             out_file_.close();
 21         }
 22     }
 23 
 24 private:
 25     void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
 26         if (!out_file_.is_open()) return;
 27 
 28         // Extract timestamp and pose information
 29         double timestamp = msg->header.stamp.toSec();
 30         double x = msg->pose.pose.position.x;
 31         double y = msg->pose.pose.position.y;
 32         double z = msg->pose.pose.position.z;
 33         double r = msg->pose.pose.orientation.x;
 34         double p = msg->pose.pose.orientation.y;
 35         double q = msg->pose.pose.orientation.z;
 36         double w = msg->pose.pose.orientation.w;
 37 
 38 
 39         // Format the output according to TUM format
 40         out_file_ << std::fixed << std::setprecision(6)
 41                   << timestamp << " "
 42                   << x << " " << y << " " << z << " "
 43                   << r << " " << p << " " << q << " "
 44                   << w << std::endl;
 45     }
 46 
 47     ros::Subscriber sub_;
 48     std::ofstream out_file_;
 49 };
 50 
 51 int main(int argc, char** argv) {
 52     ros::init(argc, argv, "odom_recorder");
 53     ros::NodeHandle nh("~");
 54 
 55     std::string filename;
 56     nh.param<std::string>("filename", filename, "odom_data.txt");
 57 
 58     OdomRecorder recorder(nh, filename);
 59 
 60     ros::spin();
 61     return 0;
 62 }

launch文件示例odom_record.launch
以下launch文件 除了启动真值tum轨迹记录节点，同步启动了录制数据包的节点rosbag。

  1 <?xml version='1.0'?>
  2 <launch>
  3 
  4   <!-- Argument to control whether to start the node -->
  5   <arg name="record" default="true" />
  6 
  7   <!-- Conditionally include the node based on the argument -->
  8   <!--Usage: roslaunch odom_recorder odom_recorder.launch record:=true-->
  9   <group if="$(arg record)">
 10     <node pkg="offboard" type="odom_recorder" name="odom_recorder" output="screen">
 11       <!-- Parameter to specify the filename -->
 12       <param name="filename" value="$(find offboard)/data/odom_data.txt"/>
 13     </node>
 14     <node pkg="rosbag" type="record" name="record_bag" args="-O $(find offboard)/data/simu.bag /camera/imu /camera/color/image_raw /camera/depth/image_raw /ground_truth/iris" />
 15   </group>
 16 
 17 </launch>

这样可以实现仿真SLAM测试，只要飞机不摔机，可以重复启动该odom_record launch和slam进行多次测试，而不需要重新加载模型。

要对比定位精度，重启odom_record launch和slam。
位置清0，只重启slam就可以，不用再起坐标系转换节点。

3）SLAM算法部署
ROS多机通讯部署，需要视景计算机与任务机处于同一局域网，具体连接方法见https://blog.csdn.net/weixin_41469272/article/details/105289174

将slam算法部署到任务机，并remap topic，同SITL。

如果要将SLAM作为无人机的定位源，需要将硬件飞控的参数进行对应修改：
param set EKF2_AID_MASK 24
param set EKF2_HGT_MODE 3

此外，可以对应修改以下参数，防止EKF2 因为vision pose 的高方差或者延时清零。如下：

延时阈值：EKF2_EV_DELAY
角度噪声阈值：EKF2_EVA_NOISE
偏移噪声阈值：EKF2_EVP_NOISE

5. 地面站部署
   地面站可以使用网络，通过mavros实现远程连接，也可以直接部署在任务机上，通过本地端口进行连接。
   见文件Offboard_square_vision_pose.launch

  <node pkg="mavros" type="mavros_node" name="mavros" output="screen">
    <param name="fcu_url" value="/dev/ttyUSB0:921600" />
    <!--param name="gcs_url" value="udp://@192.168.xxx.xxx<-your GCS IP " /-->
    <!--Rmote: -->
    <param name="gcs_url" value="udp://@192.168.1.250" />
    <!--local: -->
    或：<param name="gcs_url" value="" />
  </node>

执行步骤同SITL，先飞控gazebo、slam、offboard，而后地面站。可以通过遥控来实现控制无人机飞行，观察定位输出。自用

总结示例

1. px4 mavros eg
#cd in ~/workspace/uav_ws/src/Firmware/launch
#source [PX4-Autopilot]/Tools/setup_gazebo.bash [PX4-Autopilot] [PX4-Autopilot]/build/posix_sitl_default
roscd px4/launch
#launch uav & world
roslaunch iris_realsense_camera_mavros_hil.launch

地面站此时可以连接飞控，在Mavlink console里可以使用`mavlink status`查看连接的飞控串口对应的mavlink协议是否开启。

2. offboard eg
cd ~/workspace/uav_ws/src/Offboard_simulation/src/offboard/launch
#launch offboard localization & mission
#source ~/workspace/uav_ws/src/Offboard_simulation/devel/setup.sh
roslaunch Offboard_square_vision_pose_simulation.launch

地面站mavlink inspector中观察LOCAL_POSITION_NED的数据是否可以稳定接收数据。

3. groundtruth record
roslauch odom_record.launch

4. slam eg
cd ~/workspace/demo_tmp/src/demo_rgbd/launch
#launch slam
#source ~/workspace/demo_tmp/devel/setup.sh
roslauch roslaunch realsense_simulation.launch

重启时，需要重新插拔飞控和串口。

pxh常用命令

在 PX4 固件 v1.8.0 中，使用 PXH 终端发送指令时，你可以使用以下常见命令和 MAVLink 指令来控制飞行器：

1. 起飞:

   commander takeoff
   

   这个命令将让飞行器执行起飞操作。

2. 着陆:

   commander land
   

   这个命令会让飞行器执行着陆操作。

3. 停止飞行:

   commander stop
   

   停止当前任务并让飞行器悬停。

4. 设置飞行模式:

   set_mode <mode>
   

   例如，set_mode OFFBOARD 可以将飞行器模式设置为 Offboard。

5. 改变高度:

   param set MAV_NAV_ALTITUDE <value>
   

   设置目标高度，<value> 是你希望的高度值（单位为米）。

MAVLink 指令

1. 起飞指令 (MAV_CMD_NAV_TAKEOFF):

   mavlink_command long 11 0 0 0 0 0 0 0 0
   

   11 是 MAV_CMD_NAV_TAKEOFF 的命令码，后面跟随的 0 是参数，可以根据需要调整。

2. 着陆指令 (MAV_CMD_NAV_LAND):

   mavlink_command long 21 0 0 0 0 0 0 0 0
   

   21 是 MAV_CMD_NAV_LAND 的命令码。

3. 更改飞行模式 (MAV_CMD_DO_SET_MODE):

   mavlink_command long 176 0 0 0 0 0 0 <mode> 0
   

   其中 176 是 MAV_CMD_DO_SET_MODE 的命令码，<mode> 是要设置的飞行模式。

4. 设置目标位置 (MAV_CMD_NAV_WAYPOINT):

   mavlink_command long 16 <latitude> <longitude> <altitude> 0 0 0 0 0
   

   16 是 MAV_CMD_NAV_WAYPOINT 的命令码，后续参数是目标位置的经度、纬度和高度。

5. 悬停 (MAV_CMD_NAV_LOITER_TIME):

   mavlink_command long 17 0 0 0 0 0 0 <time> 0
   

   17 是 MAV_CMD_NAV_LOITER_TIME 的命令码，<time> 是悬停的时间（秒）。

其他

mavlink console 命令：
help 打出所有命令
param 设置参数，同手动设置参数一致
[cmd] help 打印命令cmd对应的使用方法
如ekf2 help
ekf2 start
ekf2 stop
ekf2 status

使用这些命令时的注意事项

• 确保你的飞行器处于合适的模式，以接收和执行这些命令（例如，Offboard 模式通常用于手动控制）。
• 使用命令前，请确认已正确配置 MAVLink 通道。
• 实际命令可能会有额外参数，建议查看官方文档或相关参考资料以获取详细说明。

更多问题或者需要进一步的信息，请查阅PX4 的官方文档或社区资源。

参考链接

https://docs.px4.io/main/zh/simulation/hitl.html
https://bbs.amovlab.com/forum.php?mod=viewthread&tid=486&extra=page%3D1
https://docs.px4.io/main/zh/sim_gazebo_gz/