基于 ROS2 和 PCL 库,用于将 .pcd
点云文件转换为用于 Navigation 的 pgm
栅格地图
pcd | pgm |
---|---|
-
读取指定的
.pcd
文件 -
使用Pass Through滤波器过滤点云
-
使用Radius Outlier滤波器进一步处理点云
-
将处理后的点云转换为占据栅格地图(Occupancy Grid Map)
-
将转换后的地图发布到指定ROS话题上
git clone https://gitee.com/LihanChen2004/pcd2pgm.git
ROS2 Humble(尚未在其他 ROS 版本中测试)
rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
cd /path/to/your/ros2_ws/
colcon build --symlink-install
source install/setup.sh
ros2 launch pcd2pgm pcd2pgm.launch.py
ros2 run nav2_map_server map_saver_cli -f YOUR_MAP_NAME
可以通过修改pcd2pgm/pcd.yaml
文件来配置节点的参数。
/pcd2pgm:
ros__parameters:
pcd_file: /home/lihanchen/NAVIGATION_WS/pcd2pgm/rmuc_2024.pcd # PCD文件所在目录
odom_to_lidar_odom: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [x, y, z, r, p, y] 里程计到激光雷达的坐标变换(用于变换点云)
flag_pass_through: false # 是否使用Pass Through滤波器
map_resolution: 0.05 # 地图分辨率
map_topic_name: map # 发布地图的ROS话题名
thre_radius: 0.5 # Radius Outlier滤波器半径
thre_z_max: 2.0 # Z轴最大值(用于Pass Through滤波器)
thre_z_min: 0.1 # Z轴最小值(用于Pass Through滤波器)
thres_point_count: 10 # 最小点数阈值(用于Radius Outlier滤波器)
use_sim_time: false
pcd_file: /home/lihanchen/NAVIGATION_WS/pcd2pgm/rmuc_2024.pcd
odom_to_lidar_odom: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# odom_to_lidar_odom: [-0.16, 0.0, -0.18, -0.523599, 0.0, -1.5707963]
flag_pass_through: false
map_resolution: 0.05
map_topic_name: map
thre_radius: 0.1
thre_z_max: 1.5
thre_z_min: -0.05
thres_point_count: 10
use_sim_time: false