ROS2：部署Mapping模块，Localization模块和Nav2导航模块

20260427 updated
Add mapping module

1. Mapping: Fastlio ROS2

以下内容都是以MID360为例，其他lidar，需要自行调整

1.1 准备

• PCL >= 1.8
• Eigen >= 3.3.4
• livox_ros_driver2：根据官方的教程慢慢安装即可，有问题看另一篇blog.

1.2 部署

1）先从github下载fastlio ros2版本，最好把它放在livox的workspace里，跟livox_ros_driver2放一起，不然后面编译可能出问题

cd <ros2_ws>/src # cd into a ros2 workspace folder git clone https://github.com/Ericsii/FAST_LIO_ROS2.git --recursive

2）编译

cd .. rosdep install --from-paths src --ignore-src -y colcon build --symlink-install . ./install/setup.bash # use setup.zsh if use zsh

1.3 Run

1）先启动fastlio的程序，可以先修改config.yaml文件，它在config文件夹里，里面也有不同类型lidar的配置文件，找到对应的型号。重点看看保存的地图pcd的路径map_file_path: "./test.pcd"

cd <ros2_ws> . install/setup.bash # use setup.zsh if use zsh ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml

2）然后启动mid360的数据流

ros2 launch livox_ros_driver2 msg_MID360_launch.py

这时在rviz里面已经可以看到当前的地图了，然后开始移动，就能看到实时产生的地图了，效果如下：

2. Localization: NDT算法

2.1 准备

• Map: ply, pcd格式都可以
• 配置好的雷达程序，这里以Mid360为例子，之前已经配好了，有问题看另一篇blog.
• TF tree：要有/map, /odom, /baselink等的关系，其中/map -> /odom之间的关系可以通过localization的程序获得，但是/odom -> /baselink -> … 后续这些link最好提前定义好，暂时没有可以调用ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 world map构建一下TF的关系

2.2 部署

2.2.1 安装 ndt_omp_ros2（必须依赖，加速 NDT）

先进入workspace，根据实际情况调整对应路径
cd ~/ros2_ws/src
git clone https://github.com/rsasaki0109/ndt_omp_ros2.git

2.2.2 安装 lidar_localization_ros2

git clone https://github.com/rsasaki0109/lidar_localization_ros2.git

2.2.3 安装其他依赖

sudo apt update
sudo apt install ros-humble-pcl-ros ros-humble-pcl-conversions ros-humble-tf2-ros

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install --packages-up-to ndt_omp_ros2 lidar_localization_ros2
source install/setup.bash

2.2.4 编译完成后需要修改一些参数

1）找到文件param/localization.yaml

/**: ros__parameters: registration_method: "NDT_OMP" score_threshold: 2.0 ndt_resolution: 1.0 ndt_step_size: 0.1 ndt_num_threads: 4 ndt_max_iterations: 35 transform_epsilon: 0.01 voxel_leaf_size: 0.2 scan_max_range: 100.0 scan_min_range: 1.0 scan_period: 0.1 use_pcd_map: true map_path: "" set_initial_pose: true initial_pose_x: 0.0 initial_pose_y: 0.0 initial_pose_z: 0.0 initial_pose_qx: 0.0 initial_pose_qy: 0.0 initial_pose_qz: 0.0 initial_pose_qw: 0.0 use_odom: false use_imu: false enable_debug: true enable_map_odom_tf: false global_frame_id: map odom_frame_id: odom base_frame_id: base_link enable_timer_publishing: false pose_publish_frequency: 30.0

修改：

• map_path: ""：改成当前地图的路径，pcd格式的map.PS: 每次改完这个路径，还要重新编译一下package，因为编译用的指令是colcon build --symlink-install --packages-up-to ndt_omp_ros2 lidar_localization_ros2
• enable_map_odom_tf: false：true，这里会把map -> odom的关系发布出来
• enable_timer_publishing：true
  以下这三个frame根据实际情况来调整：

global_frame_id: map odom_frame_id: odom base_frame_id: base_link

2）找到文件/launch/lidar_localization.launch.py
找到以下这段code，看看要不要修改/base_link和/velodyne，根据实际情况来，我用的是mid360，所以velodyne->livox_frame

lidar_tf=launch_ros.actions.Node(name='lidar_tf',package='tf2_ros',executable='static_transform_publisher',arguments=['0','0','0','0','0','0','1','base_link','velodyne'])

另一处要改的是以下这段code，关于点云的映射关系，/velodyne_points->/livox/lidar，改成实际雷达的点云输出即可，数据类型为sensor_msgs/PointCloud2

lidar_localization=launch_ros.actions.LifecycleNode(name='lidar_localization',namespace='',package='lidar_localization_ros2',executable='lidar_localization_node',parameters=[localization_param_dir],remappings=[('/cloud','/velodyne_points')],output='screen')

2.3.运行

• 先启动TF tree相关的节点
• 启动雷达的节点
• 确保有以下的input
  /cloud (sensor_msgs/PointCloud2)
  /map (sensor_msgs/PointCloud2)
  /initialpose (geometry_msgs/PoseStamed)这个在启动完localization节点后，点一下rviz的2D pose estimate就行了
• 先cd到localization的ws里，启动rvizrviz2 -d src/lidar_localization_ros2/rviz/localization.rviz，也可以不用它们提供的rviz配置文件，自己启动一个rviz然后添加一下对应topic就行了
• 运行ros2 launch lidar_localization_ros2 lidar_localization.launch.py
• 这时再到rviz里给个初始位置就行了

效果如下：

3. Navigation: Nav2 module

3.1 准备

• /odomtopic: 只要前面的localization module部署并运行后，能持续发布/odom这个topic就可以了
• /cmd_veltopic：提供控制小车的topic
• Map: 这里用的map跟定位的map有点区别，这里指的是pgm格式的map和对应的yaml文件，定位用的是pcd map，pgm是一个二维平面地图，现有的导航方法基本都是基于2D平面计算全局和叠加局部的实时障碍层计算可行路径（A*或Dijkstra算法）。
  这里从pcd->pgm有开源的包 pcd2pgm. 具体使用方法如下：

1）启动 pcd2pgm 节点，可在 RViz 中预览滤波后点云和栅格地图：

ros2 launch pcd2pgm pcd2pgm_launch.py

2）保存栅格地图：

ros2 run nav2_map_server map_saver_cli-f<YOUR_MAP_NAME>

3）参数调整
可以通过修改pcd2pgm/pcd2pgm.yaml文件来配置节点的参数。

pcd2pgm:ros__parameters:pcd_file:/home/xxx/pcdMap/test_map.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.1# Radius Outlier 滤波器半径thre_z_max:2.0# Z轴最大值（用于 Pass Through 滤波器）thre_z_min:0.1# Z轴最小值（用于 Pass Through 滤波器）thres_point_count:10# 最小点数阈值（用于 Radius Outlier 滤波器）

4）建议：通过上面这个包确实可以快速得到pgm地图开始导航，但是也会发现，由于扫图和建图的时候，地图里存在很多噪点没有被清除干净，使得实际的地图里，有很多本不应该存在的点，这些点都会被当做是障碍物来处理，很影响导航的效果，所以，通常在生成pgm地图前，先用一些点云处理软件，在Linux里面可以使用meshlab，非常好用，先把pcd导入，然后把天花板和地面的点给去了，不用特别精细也可以，如果要特别的精细的，写个程序，加入pcl的库，就可以精确的去除天花和地板，去除完后，就可以从俯视图里观察，是不是存在一些噪点，这是用meshlab的清除工具，一个个把不必要的噪点框出 然后删除即可，做完这一步的处理后，再进行上面的pcd转pgm，就能得到一张干干净净的导航地图了。

3.2 部署

安装Nav2 module，改成ROS2对应的版本，以下以humble为例：

sudo apt update sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

部署完之后，先去根目录那找到nav2的参数文件/opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml，比较长，内容很多，有很多可以调节的参数，这个可以细细研究，有时比较玄学。可以把这个参数复制到ws里面也可以的，不是必须要启动根目录这个参数文件。

3.3 运行

运行之前搞清楚两个文件的路径就可以了，一个是map的路径，就是上面我们准备的pgm地图的yaml文件路径，然后是导航nav2的参数文件路径，运行的指令如下：
ros2 launch nav2_bringup bringup_launch.py use_sim_time:=False autostart:=True map:=/home/xxx/ws_nav/src/Map/cp_3floor_v2_260415.yaml params_file:=/opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml

然后打开rviz2，添加tf，point cloud，map这些topic，完成初始定位后，就可以用rviz2的tool bar上面的2D goal pose功能，可以手动发布一个目标点的topic，测试成功后，可以在code里通过action:/navigate_to_pose，类型为nav2_msgs/action/NavigateToPose，然后可以通过action：/navigate_to_pose/_action/status来查看当前的导航状态，状态含义如下：

• 1 = 已接收
• 2 = 执行中
• 4 = 成功到达
• 5 = 已取消
• 6 = 失败

具体效果如下：