Docker ROS1 Diff-Planner PX4 真机部署教程
前言
本文记录在 Jetson 上通过 Docker 跑通 ROS1 真机链路:
1 | Mid-360S |
环境:
1 | 机载电脑:Jetson Orin NX |
- 宿主机只负责 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 | Jetson 宿主机 |
各层对应代码如下:
- Dockerfile 部署层:
docker/Dockerfile、docker/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.sh、config/rviz/jetson_real_stack.rviz、scripts/rviz_goal_to_diff_planner.py,负责显示点云、里程计、占据地图、规划轨迹,并把2D Nav Goal或Publish 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 | cd /home/jetson2 |
然后启动真机链路:
1 | FCU_URL='serial:///dev/ttyACM0:57600' \ |
后面的手动安装步骤主要用于理解链路和排错;实际迁移建议直接用这个 Dockerfile 仓库。
一、宿主机准备
下面命令都在 Jetson 宿主机执行。
1. Docker
1 | sudo systemctl status docker |
如当前用户没有 Docker 权限:
1 | sudo usermod -aG docker $USER |
2. Mid-360S 网口
查看网卡:
1 | ip addr |
假设雷达网卡为 eth0:
1 | sudo ip addr add 192.168.1.101/24 dev eth0 |
把上面 IP 改成你的实际值。
3. PX4 串口
1 | ls -l /dev/ttyACM* |
本文默认使用 /dev/ttyACM0。
二、创建 ROS1 容器
如果你已经有 ros_noetic 容器,可以跳过。
1 | mkdir -p ~/docker/ros_root |
创建容器:
1 | docker run -it \ |
重新进入:
1 | docker start -ai ros_noetic |
三、容器内基础环境
下面命令都在容器内执行。
安装依赖:
1 | apt update |
初始化 rosdep:
1 | rosdep init || true |
安装 GeographicLib 数据:
1 | cd ~ |
在 ~/.bashrc 末尾添加:
1 | [ -f /opt/ros/noetic/setup.bash ] && source /opt/ros/noetic/setup.bash |
生效:
1 | source ~/.bashrc |
四、安装 Livox 驱动
安装 Livox-SDK2:
1 | cd ~ |
编译 livox_ros_driver2:
1 | mkdir -p ~/livox_ws/src |
配置 Mid-360S:
1 | cd ~/livox_ws/src/livox_ros_driver2/config |
重点确认:
host_ip:Jetson 雷达网卡 IPlidar_configs[].ip:Mid-360S IP
五、编译 FAST-LIO、Diff-Planner 和工具节点
拉源码:
1 | mkdir -p ~/catkin_ws/src |
更新 FAST-LIO 子模块:
1 | cd ~/catkin_ws/src/FAST_LIO |
适配 livox_ros_driver2:
1 | cd ~/catkin_ws/src/FAST_LIO |
补消息依赖:
1 | grep -q 'add_dependencies(fastlio_mapping' CMakeLists.txt || \ |
如果 Mid-360S 相对机体不是水平安装,建议把 FAST_LIO/src/IMU_Processing.hpp 初始化逻辑改成重力对齐版本:
1 | state_ikfom init_state = kf_state.get_x(); |
当前环境使用两个已调通脚本:
1 | /root/catkin_ws/src/px4_realflight_tools/scripts/odom_to_base.py |
默认安装外参:
1 | MOUNT_X=0.0 |
含义:Mid-360S 相对机体前倾约 30 度,高于飞控约 10 cm。
编译:
1 | cd ~/catkin_ws |
六、PX4 参数
在 QGroundControl 中重点确认:
1 | MAV_1_CONFIG = TELEM 2 |
七、一键启动脚本
Jetson 宿主机脚本路径:
1 | /home/jetson2/diff-planner-px4-deployment/scripts/start_real_px4_mid360_fastlio.sh |
它会自动启动:
livox_ros_driver2fast_lioodom_to_base.pymavrosodom_to_pose.pydiff_plannertrajectory_msg_converter.pyse3_controller
常用命令:
1 | cd /home/jetson2/diff-planner-px4-deployment |
现在的 restart 会先执行完整 stop,尽量清理 tmux 会话和容器残留进程,再重新启动整条链路。
当前真机实测可用参数
当前这台机体较稳定的一组参数:
1 | FCU_URL='serial:///dev/ttyACM0:57600' \ |
如果切 Offboard 仍掉高,优先微调:
SE3_HOVER_PERCENT:每次调0.02 ~ 0.03DIFF_PLANNER_INFLATION_SIZE
如果只想测试定位和 PX4 外部视觉融合:
1 | START_DIFF_PLANNER=false \ |
八、快速检查
进入容器后,最少检查下面几项:
1 | source ~/.bashrc |
只要满足下面几点,链路通常就已经通了:
/livox/lidar类型为livox_ros_driver2/CustomMsg/Odometry、/Odometry_base、/mavros/vision_pose/pose持续输出/mavros/state中connected: True/drone_0_planning/pos_cmd和/mavros/setpoint_raw/attitude在发目标后有输出
飞行原则也保持最简单:
- 脚本不会自动解锁
- 脚本不会自动切 Offboard
- 先用遥控器起飞并悬停,再发目标点,再手动切 Offboard
- 飞行中可随时切回 Position 接管
九、常见问题
- 容器里没有
/dev/ttyACM0:重建容器时确认加入--device=/dev/ttyACM0 - MAVROS 未连接:检查
/dev/ttyACM0、FCU_URL、/mavros/state - 视觉位姿有数据但 PX4 不融合:检查
EKF2_EV_CTRL、EKF2_HGT_REF和 QGC EKF 告警 - 切 Offboard 立刻退出:检查
/mavros/setpoint_raw/attitude是否持续发布,并确认COM_OF_LOSS_T、COM_OBL_RC_ACT - Offboard 掉高:优先微调
SE3_HOVER_PERCENT、SE3_MAX_HOVER_PERCENT、SE3_MAX_OUTPUT_THRUST
参考资料
- PX4 外部视觉定位文档:https://docs.px4.io/main/en/ros/external_position_estimation.html
- PX4 Offboard 模式文档:https://docs.px4.io/main/en/flight_modes/offboard.html
- MAVROS 文档:https://mavros.readthedocs.io/en/latest/
- 本文 Dockerfile 部署仓库:https://github.com/dreamer198/diff-planner-px4-deployment
- Livox ROS Driver 2:https://github.com/Livox-SDK/livox_ros_driver2
- FAST-LIO:https://github.com/hku-mars/FAST_LIO
- Diff-Planner-PX4:https://github.com/dreamer198/Diff-Planner-PX4