ROS move_base 混合功能導航 RealSense D435i + 3D 點云地圖 + 樓層切換 + 路徑錄制 + 路徑規劃

Mixed-Navigation

這個博客也是記錄我們的一個開源項目，其作用是混合功能導航。由于現有的 Fast-Lio-Localization 只實現了定位功能，但對于路徑規劃和樓層切換沒有具體實現，因此我們開出了這個倉庫作為參考。該倉庫的核心功能如下：

可以使用 RealSense D435i 相機的點云作為局部代價地圖；
可以使用激光雷達 PointCloud2 格式作為局部代價地圖；
對任意高度的 3D 點云地圖抽取指定高度范圍作為 2D 全局地圖；
對不同樓層的地圖實現了管理與服務調用式的切換；
可以在 Rviz 上錄制不同路徑導航點；
同一個樓層允許添加多個導航路徑；

我們建議你直接跳轉我們的開源倉庫，所有功能更新與 Bug 修復都會在第一時間同步到倉庫中，博客的更新可能存在滯后。

Mixed Navigation: https://github.com/GaohaoZhou-ops/Mixed-Navigation/tree/main#

這篇博客剩余的內容就直接復制項目中 ReadMe-CN.md 文件。

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
$ sudo apt-get install libopenvdb-dev
$ sudo apt-get install ros-noetic-teb-local-planner

conda 環境

因為將點云文件轉化為 2D 柵格地圖需要一些 python 的庫，建議在新建一個 conda 環境進行操作以避免影響到本機環境。

$ conda create -n mapconv python=3.10
$ conda activate mapconv
$ pip install "numpy<2.0"
$ pip install cython open3d PyYAML Pillow commentjson pyntcloud rospkg

Step2. 源碼編譯

拉取源碼

在安裝完依賴庫后使用下面的命令編譯源碼，假設你的工作空間名字是 nav_ws：

$ cd nav_ws/src
$ git clone https://github.com/GaohaoZhou-ops/Mixed-Navigation.git

因為這里沒有對另外兩個包中的源碼進行修改，所以在拉取后還需要切換一下分支：

$ cd nav_ws/src/
$ git clone https://github.com/IntelRealSense/realsense-ros.git
$ cd realsense-ros
$ git checkout ros1-legacy$ cd nav_ws/src/
$ git clone https://github.com/SteveMacenski/spatio_temporal_voxel_layer.git
$ cd 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/resources/floors 包中的結構如下所示：

$ cd src/mixed_nav/resources/floors
.
├── floor0
│   ├── floor0.pgm
│   ├── floor0.yaml
│   └── waypoints.json
├── floor1
│   ├── floor1.pgm
│   ├── floor1.yaml
│   └── waypoints.json
├── floor3
│   ├── floor3.pgm
│   ├── floor3.yaml
│   └── waypoints.json
├── room_zheng.ply      # 下載好的點云地圖文件
└── z_config.json

使用自己現成的地圖文件

如果你已經有了一份點云地圖文件，將其移動到 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/tools/pc2pgm.py 并結合可選參數將點云地圖文件轉換成 2D 柵格地圖，假設你的地圖名為 room_zheng.ply，下面命令執行后會將點云文件中高度在 $[0.1, 0.8]$ 范圍內的點云壓縮成一個 2D 地圖，并保存到 resources/floors 文件夾下：

$ python scripts/pc2pgm.py resources/room_zheng.ply  --floor_name floor0 --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 查看具體功能。

【可選】使用下面的命令查看一下生成的點云地圖是否滿足你的需求，如果不滿足可以通過修改 $Z$ 軸的高度范圍重新生成：

下面的命令會同時加載一份 3D 點云地圖，由于點云文件通常比較大，因此可能會消耗一定時間，如果你不想加載點云文件可以修改 map_server.launch 文件中的參數配置取消顯示：

$ cd nav_ws
$ source devel/setup.bash 
$ roslaunch mixed_nav map_server.launch

在這里插入圖片描述

Step5. 設置導航點

在完成地圖編輯后使用下面的命令啟動導航點記錄節點，你需要在 src/Mixed-Navigation/mixed_nav/launch/record_nav_points.launch 文件中明確指出當前地圖所在的樓層 floor_name 以讓 map_server 加載地圖信息：

$ cd nav_ws
$ source devel/setup.bash 
$ roslaunch mixed_nav record_nav_points.launch

記錄節點每次啟動后可以錄制多條導航軌跡，新開一個終端并調用服務 /start_record_nav_point 來告知節點當前路徑名，這里假設為 path_alpha

$ cd nav_ws
$ source devel/setup.bash 
$ rosservice call /start_record_nav_point "path_name: 'path_alpha'"

然后在 rviz 窗口中使用 2D Nav Goal 按鈕按照順序逐個創建導航點；如果你在錄制過程中操作失誤，可以調用服務 /undo_record_nav_point 取消最近一次記錄。

當一條路徑錄制結束后調用服務 /finish_record_nav_point 告知當前結束錄制。
在這里插入圖片描述

最終得到的路徑會保存到 src/Mixed-Navigation/mixed_nav/resources/floors/floor_name/waypoints.json 文件中：

{"path_alpha": [{"position": {"x": 1.382780909538269,"y": 3.176682710647583,"z": 0.0},"orientation": {"x": 0.0,"y": 0.0,"z": 0.0114526433511636,"w": 0.9999344163295266}},],// ..."path_beta": [{"position": {"x": 3.6949002742767334,"y": 3.6009724140167236,"z": 0.0},"orientation": {"x": 0.0,"y": 0.0,"z": 0.011048003717325597,"w": 0.9999389689445362}},]}

Step6. 啟動仿真導航

如果你已經安裝好 RealSense SDK 那么可以順便啟動相機，這樣能夠將點云映射到局部代價地圖上以實現更動態的導航避障，如果你目前沒有連接相機也不會影響仿真：

$ cd nav_ws
$ source devel/setup.bash 
$ roslaunch realsense2_camera rs_rgbd.launch

在這里插入圖片描述

在仿真 rviz 界面中使用 2D Pose Estimate 按鈕給一個初試姿態如下圖所示：
在這里插入圖片描述

使用下面的命令啟動仿真導航：

$ cd nav_ws
$ source devel/setup.bash 
$ roslaunch mixed_nav navigation_sim.launch

功能一：自由路徑導航

在rviz中提供一個 2D Nav Goal 可以模擬自由導航到目的地：
在這里插入圖片描述

功能二：逐點導航

該功能需要確保 waypoints 文件夾中的導航路徑文件存在且內容無誤，使用下面的命令朝 /track_points/goal 話題發布一個路徑組名 path_group_name 以及死區半徑 dead_zone_radius 可以讓其自動沿著路徑組 path_beta 進行逐點導航。本質是逐點發布導航點到 /move_base_simple/goal 話題中讓 move_base 完成路徑規劃功能，期間實時監聽 base_link 是否到達死區內，如果到達則發布下一個點

$ rostopic pub /track_points/goal mixed_nav/PathNavigationActionGoal "header:seq: 0stamp:secs: 0nsecs: 0frame_id: ''
goal_id:stamp:secs: 0nsecs: 0id: ''
goal:path_group_name: 'path_beta'dead_zone_radius: 0.2"

在這里插入圖片描述

Step7. 切換地圖

如果你想要使用不同高度或樓層作為導航地圖，使用下面的命令可以進行地圖切換，切換成功后對應的地圖點云文件也會同步修改：

$ cd nav_ws
$ source devel/setup.bash
$ rosservice call /switch_floor "floor_name": 'floor0'

floor0	floor1	floor3