前言

本文记录在 Jetson 上通过 Docker 跑通 ROS1 真机链路:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
Mid-360S
-> livox_ros_driver2
-> FAST-LIO
-> /Odometry + /cloud_registered
-> odom_to_base.py
-> /Odometry_base
-> odom_to_pose.py
-> /mavros/vision_pose/pose
-> PX4 EKF2

/Odometry_base + /cloud_registered
-> Diff-Planner
-> /drone_0_planning/pos_cmd
-> trajectory_msg_converter.py
-> /command/trajectory
-> se3_controller
-> /mavros/setpoint_raw/attitude
-> PX4 Offboard

环境:

1
2
3
4
5
6
机载电脑:Jetson Orin NX
宿主系统:Ubuntu 22.04
容器镜像:ros:noetic-ros-base-focal
飞控:PX4
雷达:Livox Mid-360S
算法:FAST-LIO + Diff-Planner
  • 宿主机只负责 Docker、雷达网口、PX4 串口。
  • ROS1 全部放进容器。
  • 真机默认不自动切 Offboard,不自动解锁,始终保留遥控器接管能力。
  • 第一次建议先不装桨;装桨后先在 0.3 ~ 0.5 m 高度测试。
  • 最新 Dockerfile 部署仓库:https://github.com/dreamer198/diff-planner-px4-deployment

代码链路总览

这套真机方案分成三部分:Jetson 宿主机负责启动和硬件访问,ROS1 节点跑在容器内,本机负责 RViz 可视化和发目标点。

目前推荐直接使用整理好的 Dockerfile 部署仓库:

1
https://github.com/dreamer198/diff-planner-px4-deployment
  • Jetson 推荐路径:/home/jetson2/diff-planner-px4-deployment
  • Jetson 宿主机启动入口:scripts/start_real_px4_mid360_fastlio.sh
  • 本机 RViz 启动入口:scripts/start_jetson_ros1_rviz.sh
  • ROS1 容器名:ros_noetic_realflight
  • 容器内工作区:~/livox_ws(Livox 驱动)、~/catkin_ws(FAST-LIO、Diff-Planner、控制相关代码)

启动脚本运行在 Jetson 宿主机,不在容器内执行。它通过 tmux + docker exec 依次拉起下面这条链路:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
Jetson 宿主机
-> scripts/start_real_px4_mid360_fastlio.sh
-> docker exec 进入 ros_noetic_realflight 容器

容器内感知链路:
livox_ros_driver2
-> /livox/lidar + /livox/imu
FAST-LIO
-> /Odometry + /cloud_registered
odom_to_base.py
-> /Odometry_base
odom_to_pose.py
-> /mavros/vision_pose/pose
MAVROS
-> PX4 EKF2

容器内规划控制链路:
/Odometry_base + 点云/地图
-> Diff-Planner
-> /drone_0_planning/pos_cmd
-> trajectory_msg_converter.py
-> /command/trajectory
-> se3_controller
-> /mavros/setpoint_raw/attitude
-> PX4 Offboard

本机可视化链路:
scripts/start_jetson_ros1_rviz.sh
-> 本机容器内 RViz + /root/jetson_real_stack.rviz + rviz_goal_to_diff_planner.py
-> /move_base_simple/goal 或 /clicked_point
-> /goal
-> Diff-Planner

各层对应代码如下:

  • Dockerfile 部署层:docker/Dockerfiledocker/docker_run_real.sh,负责构建镜像、创建 ros_noetic_realflight 容器并挂载真机配置。
  • 启动调度层:scripts/start_real_px4_mid360_fastlio.sh,负责 start / stop / restart / status / attach,并在宿主机顺序拉起整条链路。
  • 雷达驱动层:~/livox_ws/src/livox_ros_driver2,入口 msg_MID360s.launch,配置 config/MID360s_config.json,发布 /livox/lidar/livox/imu
  • 里程计层:~/catkin_ws/src/FAST_LIO,入口 launch/mapping_mid360.launch,发布 /Odometry、`/cloud_registered``;Mid-360S 倾斜安装时常改 IMU 初始化逻辑。
  • 机体外参补偿层:/root/catkin_ws/src/px4_realflight_tools/scripts/odom_to_base.py,把 /Odometry 转成 /Odometry_base
  • PX4 视觉桥接层:/root/catkin_ws/src/px4_realflight_tools/scripts/odom_to_pose.py,把 /Odometry_base 转成 /mavros/vision_pose/pose
  • 规划层:~/catkin_ws/src/Diff-Planner-PX4/src/diff_planner,真机入口通常是 run_real_mid360_lio.launch,输出 /drone_0_planning/pos_cmd
  • 轨迹格式转换层:~/catkin_ws/src/Diff-Planner-PX4/src/diff_planner/plan_manage/scripts/trajectory_msg_converter.py,把 /drone_0_planning/pos_cmd 转成 /command/trajectory
  • 控制层:~/catkin_ws/src/Diff-Planner-PX4/src/se3_controller,核心节点 src/se3_controller_node.cpp,把 /command/trajectory 转成 /mavros/setpoint_raw/attitude
  • 本机 RViz 层:scripts/start_jetson_ros1_rviz.shconfig/rviz/jetson_real_stack.rvizscripts/rviz_goal_to_diff_planner.py,负责显示点云、里程计、占据地图、规划轨迹,并把 2D Nav GoalPublish Point 转成 /goal

如果后面要排错,可以按这个顺序看:/livox/lidar -> /Odometry -> /Odometry_base -> /mavros/vision_pose/pose -> /drone_0_planning/pos_cmd -> /command/trajectory -> /mavros/setpoint_raw/attitude

推荐:使用 Dockerfile 部署仓库

新 Jetson 迁移时,优先使用这个仓库,而不是手动一步步复制代码:

1
2
3
4
5
cd /home/jetson2
git clone https://github.com/dreamer198/diff-planner-px4-deployment.git
cd diff-planner-px4-deployment
./docker/docker_run_real.sh build
FCU_DEVICE=/dev/ttyACM0 ./docker/docker_run_real.sh restart

然后启动真机链路:

1
2
3
4
5
6
7
8
9
FCU_URL='serial:///dev/ttyACM0:57600' \
GCS_URL='udp://:14550@10.0.30.196:14550' \
SE3_HOVER_PERCENT=0.90 \
SE3_MAX_HOVER_PERCENT=0.95 \
SE3_MAX_OUTPUT_THRUST=1.00 \
SE3_ENABLE_THRUST_ESTIMATION=false \
SE3_MAX_FEEDFORWARD_ACC=1.2 \
DIFF_PLANNER_INFLATION_SIZE=0.2 \
./scripts/start_real_px4_mid360_fastlio.sh start

后面的手动安装步骤主要用于理解链路和排错;实际迁移建议直接用这个 Dockerfile 仓库。

一、宿主机准备

下面命令都在 Jetson 宿主机执行。

1. Docker

1
2
sudo systemctl status docker
docker info

如当前用户没有 Docker 权限:

1
sudo usermod -aG docker $USER

2. Mid-360S 网口

查看网卡:

1
ip addr

假设雷达网卡为 eth0

1
2
3
sudo ip addr add 192.168.1.101/24 dev eth0
sudo ip link set eth0 up
ping 192.168.1.199

把上面 IP 改成你的实际值。

3. PX4 串口

1
2
ls -l /dev/ttyACM*
sudo chmod 666 /dev/ttyACM0

本文默认使用 /dev/ttyACM0

二、创建 ROS1 容器

如果你已经有 ros_noetic 容器,可以跳过。

1
2
mkdir -p ~/docker/ros_root
xhost +SI:localuser:root

创建容器:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
docker run -it \
--name ros_noetic \
--network host \
--ipc host \
--privileged \
--device=/dev/ttyACM0 \
-e DISPLAY=$DISPLAY \
-e XAUTHORITY=/root/.Xauthority \
-e QT_X11_NO_MITSHM=1 \
-v /tmp/.X11-unix:/tmp/.X11-unix \
-v $HOME/.Xauthority:/root/.Xauthority:ro \
-v /dev/bus/usb:/dev/bus/usb \
-v ~/docker/ros_root:/root \
ros:noetic-ros-base-focal \
bash

重新进入:

1
2
docker start -ai ros_noetic
docker exec -it ros_noetic bash

三、容器内基础环境

下面命令都在容器内执行。

安装依赖:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
apt update
apt install -y \
git wget curl vim nano \
net-tools iproute2 iputils-ping \
build-essential cmake python3-pip \
python3-catkin-tools python3-rosdep python3-osrf-pycommon \
libeigen3-dev libpcl-dev pcl-tools libopencv-dev \
libgoogle-glog-dev libgflags-dev libarmadillo-dev \
ros-noetic-cv-bridge ros-noetic-eigen-conversions \
ros-noetic-mavros ros-noetic-mavros-extras \
ros-noetic-pcl-ros ros-noetic-roslint \
ros-noetic-tf ros-noetic-tf2-ros \
ros-noetic-tf2-sensor-msgs ros-noetic-tf2-geometry-msgs \
ros-noetic-ddynamic-reconfigure ros-noetic-rviz

初始化 rosdep:

1
2
rosdep init || true
rosdep update

安装 GeographicLib 数据:

1
2
3
4
cd ~
wget https://gitee.com/tyx6/mytools/raw/main/mavros/install_geographiclib_datasets.sh
chmod +x install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh

~/.bashrc 末尾添加:

1
2
3
4
5
6
7
[ -f /opt/ros/noetic/setup.bash ] && source /opt/ros/noetic/setup.bash
[ -f ~/livox_ws/devel/setup.bash ] && source ~/livox_ws/devel/setup.bash
[ -f ~/catkin_ws/devel/setup.bash ] && source ~/catkin_ws/devel/setup.bash

export FCU_URL="serial:///dev/ttyACM0:57600"
export GCS_URL="udp://:14550@10.0.30.196:14550"
export DRONE_ID=0

生效:

1
source ~/.bashrc

四、安装 Livox 驱动

安装 Livox-SDK2:

1
2
3
4
5
6
7
8
9
10
cd ~
mkdir -p ~/code
cd ~/code
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2
mkdir -p build && cd build
cmake ..
make -j4
make install
ldconfig

编译 livox_ros_driver2

1
2
3
4
5
6
7
mkdir -p ~/livox_ws/src
cd ~/livox_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
cd ~/livox_ws/src/livox_ros_driver2
source /opt/ros/noetic/setup.bash
./build.sh ROS1
source ~/livox_ws/devel/setup.bash

配置 Mid-360S:

1
2
cd ~/livox_ws/src/livox_ros_driver2/config
nano MID360s_config.json

重点确认:

  • host_ip:Jetson 雷达网卡 IP
  • lidar_configs[].ip:Mid-360S IP

五、编译 FAST-LIO、Diff-Planner 和工具节点

拉源码:

1
2
3
4
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone --recursive https://github.com/hku-mars/FAST_LIO.git
git clone -b px4_sitl https://github.com/dreamer198/Diff-Planner-PX4.git

更新 FAST-LIO 子模块:

1
2
cd ~/catkin_ws/src/FAST_LIO
git submodule update --init --recursive

适配 livox_ros_driver2

1
2
3
4
5
6
7
cd ~/catkin_ws/src/FAST_LIO
sed -i -E 's/livox_ros_driver([^2]|$)/livox_ros_driver2\1/g' \
CMakeLists.txt \
package.xml \
src/laserMapping.cpp \
src/preprocess.cpp \
src/preprocess.h

补消息依赖:

1
2
grep -q 'add_dependencies(fastlio_mapping' CMakeLists.txt || \
sed -i '/add_executable(fastlio_mapping/a add_dependencies(fastlio_mapping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})' CMakeLists.txt

如果 Mid-360S 相对机体不是水平安装,建议把 FAST_LIO/src/IMU_Processing.hpp 初始化逻辑改成重力对齐版本:

1
2
3
4
5
6
7
state_ikfom init_state = kf_state.get_x();
init_state.grav = S2(V3D(0, 0, -G_m_s2));
Eigen::Quaterniond q_init = Eigen::Quaterniond::FromTwoVectors(mean_acc, V3D(0, 0, 1));
init_state.rot = SO3(q_init.w(), q_init.x(), q_init.y(), q_init.z());
init_state.bg = mean_gyr;
init_state.offset_T_L_I = Lidar_T_wrt_IMU;
init_state.offset_R_L_I = Lidar_R_wrt_IMU;

当前环境使用两个已调通脚本:

1
2
/root/catkin_ws/src/px4_realflight_tools/scripts/odom_to_base.py
/root/catkin_ws/src/px4_realflight_tools/scripts/odom_to_pose.py

默认安装外参:

1
2
3
4
5
6
MOUNT_X=0.0
MOUNT_Y=0.0
MOUNT_Z=0.10
MOUNT_ROLL_DEG=0.0
MOUNT_PITCH_DEG=30.0
MOUNT_YAW_DEG=0.0

含义:Mid-360S 相对机体前倾约 30 度,高于飞控约 10 cm。

编译:

1
2
3
4
5
6
7
8
cd ~/catkin_ws
source /opt/ros/noetic/setup.bash
source ~/livox_ws/devel/setup.bash
catkin init
catkin config --extend ~/livox_ws/devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build
source ~/catkin_ws/devel/setup.bash

六、PX4 参数

在 QGroundControl 中重点确认:

1
2
3
4
5
6
7
MAV_1_CONFIG = TELEM 2
MAV_1_MODE = Onboard
SER_TEL2_BAUD = 921600
EKF2_EV_CTRL = 15
EKF2_HGT_REF = Vision
EKF2_EV_DELAY = 0.0
EKF2_EV_POS_X/Y/Z = 0.0

七、一键启动脚本

Jetson 宿主机脚本路径:

1
/home/jetson2/diff-planner-px4-deployment/scripts/start_real_px4_mid360_fastlio.sh

它会自动启动:

  • livox_ros_driver2
  • fast_lio
  • odom_to_base.py
  • mavros
  • odom_to_pose.py
  • diff_planner
  • trajectory_msg_converter.py
  • se3_controller

常用命令:

1
2
3
4
5
6
cd /home/jetson2/diff-planner-px4-deployment
./scripts/start_real_px4_mid360_fastlio.sh start
./scripts/start_real_px4_mid360_fastlio.sh status
./scripts/start_real_px4_mid360_fastlio.sh attach
./scripts/start_real_px4_mid360_fastlio.sh stop
./scripts/start_real_px4_mid360_fastlio.sh restart

现在的 restart 会先执行完整 stop,尽量清理 tmux 会话和容器残留进程,再重新启动整条链路。

当前真机实测可用参数

当前这台机体较稳定的一组参数:

1
2
3
4
5
6
7
8
9
FCU_URL='serial:///dev/ttyACM0:57600' \
GCS_URL='udp://:14550@10.0.30.196:14550' \
SE3_HOVER_PERCENT=0.90 \
SE3_MAX_HOVER_PERCENT=0.95 \
SE3_MAX_OUTPUT_THRUST=1.00 \
SE3_ENABLE_THRUST_ESTIMATION=false \
SE3_MAX_FEEDFORWARD_ACC=1.2 \
DIFF_PLANNER_INFLATION_SIZE=0.2 \
./scripts/start_real_px4_mid360_fastlio.sh restart

如果切 Offboard 仍掉高,优先微调:

  • SE3_HOVER_PERCENT:每次调 0.02 ~ 0.03
  • DIFF_PLANNER_INFLATION_SIZE

如果只想测试定位和 PX4 外部视觉融合:

1
2
START_DIFF_PLANNER=false \
./scripts/start_real_px4_mid360_fastlio.sh restart

八、快速检查

进入容器后,最少检查下面几项:

1
2
3
4
5
6
7
8
source ~/.bashrc
rostopic type /livox/lidar
rostopic hz /Odometry
rostopic hz /Odometry_base
rostopic echo -n 1 /mavros/state
rostopic hz /mavros/vision_pose/pose
rostopic hz /drone_0_planning/pos_cmd
rostopic hz /mavros/setpoint_raw/attitude

只要满足下面几点,链路通常就已经通了:

  • /livox/lidar 类型为 livox_ros_driver2/CustomMsg
  • /Odometry/Odometry_base/mavros/vision_pose/pose 持续输出
  • /mavros/stateconnected: True
  • /drone_0_planning/pos_cmd/mavros/setpoint_raw/attitude 在发目标后有输出

飞行原则也保持最简单:

  • 脚本不会自动解锁
  • 脚本不会自动切 Offboard
  • 先用遥控器起飞并悬停,再发目标点,再手动切 Offboard
  • 飞行中可随时切回 Position 接管

九、常见问题

  • 容器里没有 /dev/ttyACM0:重建容器时确认加入 --device=/dev/ttyACM0
  • MAVROS 未连接:检查 /dev/ttyACM0FCU_URL/mavros/state
  • 视觉位姿有数据但 PX4 不融合:检查 EKF2_EV_CTRLEKF2_HGT_REF 和 QGC EKF 告警
  • 切 Offboard 立刻退出:检查 /mavros/setpoint_raw/attitude 是否持续发布,并确认 COM_OF_LOSS_TCOM_OBL_RC_ACT
  • Offboard 掉高:优先微调 SE3_HOVER_PERCENTSE3_MAX_HOVER_PERCENTSE3_MAX_OUTPUT_THRUST

参考资料