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里面已经可以看到当前的地图了，然后开始移动，就能看到实时产生的地图了，效果如下：
Fastlio ROS2 - test
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_ros2enable_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里给个初始位置就行了
效果如下：
Localization Test-1
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 = 失败
具体效果如下：
ROS2-nav2
扫一扫
1336
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
