Nvidia Orin + RealSense D435i 与3D地图实现导航
这个博客是我的一个开源项目的同步版，实现了在 Nvidia Orin 上用 RealSense D435i 作为局部代价地图、3D 点云地图抽取指定高度作为 2D 地图的 move_base 路径规划。
- Github 项目链接：https://github.com/GaohaoZhou-ops/Mixed-Navigation
这个项目会持续维护，但会优先更新到 Github 上，建议直接看 Github 上的内容。如果你遇到了什么问题在这篇博客和 Github 上留言都是等效的，我看到了会一样处理。
下面的部分是直接照搬我项目的 ReadMe 文件，如果有出现重复的描述还请见谅。
Mixed Nav
这个包是一个混合导航demo，将3D点云文件抽取指定高度映射成 2D 地图文件，并用到了 RealSense D435i、Spatio Temporal Voxel Layer、move_base 等插件，并提供了仿真和真机两种启动方式。目的是和我的另一个开源项目结合使用，实现用 Mid360 + Fast-Lio 魔改版的建图、定位、导航。
代码中涉及到的开源仓库如下所示：
- RealSense-ROS：https://github.com/IntelRealSense/realsense-ros
- Spatio-Temporal Voxel Layer：https://github.com/SteveMacenski/spatio_temporal_voxel_layer
代码在以下环境中通过了测试，更多的平台测试将持续推出：
| Device | Plantform | OS | ROS |
|---|---|---|---|
| Nvidia Orin DK | Arm | Ubuntu 20.04 | Noetic |
这个工程中涉及到的部分资源可以在下面的网盘链接中找到：
https://pan.baidu.com/s/1N1MpEK--n21-cuk45Z0BBA?pwd=sf9y
Step1. 安装依赖
基础依赖库
在你的环境中需要安装不限于以下几个依赖：
$ sudo apt-get install ros-noetic-navigation ros-noetic-tf2-sensor-msgs libopenvdb-dev
conda 环境
因为将点云文件转化为 2D 栅格地图需要一些 python 的库，建议在新建一个 conda 环境进行操作以避免影响到本机环境。
$ conda create -n mapconv python=3.10
$ conda activate mapconv
$ pip install numpy open3d PyYAML Pillow
Step2. 源码编译
拉取源码
在安装完依赖库后使用下面的命令编译源码，假设你的工作空间名字是 nav_ws：
$ cd nav_ws/src
$ git clone --recursive git@github.com:GaohaoZhou-ops/Mixed-Navigation.git
因为这里没有对另外两个包中的源码进行修改，所以在拉取后还需要切换一下分支：
$ cd nav_ws/src/realsense-ros
$ git checkout ros1-legacy$ cd nav_ws/src/spatio_temporal_voxel_layer
$ git checkout noetic-devel
修改 realsense-ros 源码
需要对 realsense-ros/realsense2_camera/CMakeLists.txt 文件进行修改，主要是添加 OpenCV 相关的部分：
find_package(OpenCV REQUIRED) # 添加 OpenCV 库
find_package(catkin REQUIRED COMPONENTSmessage_generationnav_msgsroscppsensor_msgsstd_msgsstd_srvsnodeletcv_bridgeimage_transporttfddynamic_reconfigurediagnostic_updaterOpenCV REQUIRED # 添加 OpenCV 库)...include_directories(include${realsense2_INCLUDE_DIR}${catkin_INCLUDE_DIRS}${OpenCV_INCLUDE_DIRS} # 添加 OpenCV 头文件目录)...target_link_libraries(${PROJECT_NAME}${realsense2_LIBRARY}${catkin_LIBRARIES}${CMAKE_THREAD_LIBS_INIT}${OpenCV_LIBRARIES} # 添加 OpenCV 库文件目录)然后使用下面的命令编译源码：
$ cd nav_ws
$ catkin_make
Step3. 获取地图点云文件
使用示例地图文件
如果你暂时还没有完成建图，可以从上面的网盘链接中下载名为 room_zheng.ply 的点云地图文件并将其移动到 mixed_nav/resources 位置：
此时你的工程目录结构应该如下所示：
$ cd nav_ws
$ tree -L 2└── src├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake├── mixed_nav├── ReadMe.md├── realsense-ros└── spatio_temporal_voxel_layer其中 mixed_nav 包中的结构如下所示：
$ cd src/mixed_nav
$ tree
.
├── CMakeLists.txt
├── config
│ ├── costmap_common_params.yaml
│ ├── dwa_local_planner_params.yaml
│ ├── global_costmap_params.yaml
│ └── local_costmap_params.yaml
├── include
│ └── mixed_nav
├── launch
│ ├── navigation_sim.launch
│ └── view_map.launch
├── package.xml
├── resources
│ ├── 2d_room_zheng.pgm
│ ├── 2d_room_zheng.yaml
│ ├── images
│ └── room_zheng.ply # 下载好的点云地图
├── rviz
│ ├── nav_rviz.rviz
│ └── view_map.rviz
├── scripts
│ ├── pc2pgm.py
│ └── simulation.py
└── src
使用自己现成的地图文件
如果你已经有了一份点云地图文件，将其移动到 src/mixed_nav/resources 目录中即可。
使用 Fast-Lio 进行建图
可以参考我另外一个仓库用 Fast-Lio 进行建图，完成建图后将点云文件移动到 src/mixed_nav/resources 目录中即可。
【待补充】
Step4. 转换点云地图文件
进入到 nav_ws/src/mixed_nav 目录下并激活 conda 环境：
$ cd nav_ws/src/mixed_nav
$ conda activate mapconv
然后使用脚本 scripts/pc2pgm.py 并结合可选参数将点云地图文件转换成 2D 栅格地图，假设你的地图名为 room_zheng.ply，下面命令执行后会将点云文件中高度在 [0.1,0.8][0.1,0.8][0.1,0.8] 范围内的点云压缩成一个 2D 地图，并保存到 resources 文件夹下：
$ python scripts/pc2pgm.py resources/room_zheng.ply --min_z 0.1 --max_z 0.8
成功运行的输入示例如下：
未指定 --map_name，使用默认名称: 2d_room_zheng
正在加载点云文件: resources/room_zheng.ply...
点云加载成功，包含 17734527 个点。
正在根据高度范围过滤点云 (min_z: -0.7 m, max_z: 0.2 m)...
过滤后剩余 3463479 个点。
计算出的地图尺寸: 634 x 194 像素。
正在保存PGM地图文件到: resources/2d_room_zheng.pgm
/home/orin/Desktop/nav_ws/src/mixed_nav/scripts/pc2pgm.py:70: DeprecationWarning: 'mode' parameter is deprecated and will be removed in Pillow 13 (2026-10-15)img = Image.fromarray(map_data, mode='L')
正在保存YAML配置文件到: resources/2d_room_zheng.yaml处理完成！
地图文件: resources/2d_room_zheng.pgm
配置文件: resources/2d_room_zheng.yaml您现在可以使用ROS map_server加载此地图:
rosrun map_server map_server /home/orin/Desktop/nav_ws/src/mixed_nav/resources/2d_room_zheng.yaml
该脚本提供了其他几个可选参数，使用 --help 查看具体功能。
【可选】使用下面的命令查看一下生成的点云地图是否满足你的需求，如果不满足可以通过修改 ZZZ 轴的高度范围重新生成：
$ cd nav_ws
$ source devel/setup.bash
$ roslaunch mixed_nav view_map.launch
Step5. 启动仿真导航
使用下面的命令启动仿真导航：
$ cd nav_ws
$ source devel/setup.bash
$ roslaunch mixed_nav navigation_sim.launch
如果你已经安装好 RealSense SDK 那么可以顺便启动相机，这样能够将点云映射到局部代价地图上以实现更动态的导航避障，如果你目前没有连接相机也不会影响仿真：
$ cd nav_ws
$ source devel/setup.bash
$ roslaunch realsense2_camera rs_rgbd.launch
在仿真 rviz 界面中使用 2D Pose Estimate 按钮给一个初试姿态如下图所示：
再提供一个 2D Nav Goal 可以模拟导航目的地：
Step6. 使用真机导航
【未完待续】