基于move_base的机器狗定位与导航

liiiuzy2026-01-07 14:43

1、fast_lio2建图与重定位

整体安装和使用流程可以参考另一篇文章fast_lio2建图与重定位
这里我先给他下采样一下，把点从950w减少到18w，
但使用cloudcompare下采样后保存的地图会导致fast_lio_localization读取点云出错：
因此需要对fast_lio_localization的源码scripts/global_localization.py

中的msg_to_array函数进行简单修改：

python 复制代码

def msg_to_array(pc_msg):
    try:
        pc_array = ros_numpy.numpify(pc_msg)
        pc = np.zeros([len(pc_array), 3])
        pc[:, 0] = pc_array['x']
        pc[:, 1] = pc_array['y']
        pc[:, 2] = pc_array['z']
        return pc
    except Exception as e:
        # 如果ros_numpy失败，改用point_cloud2
        import sensor_msgs.point_cloud2 as pc2
        points = list(pc2.read_points(pc_msg, field_names=("x", "y", "z"), skip_nans=True))
        return np.array(points)

接下来重定位就没有问题了

这里map和camera_Init之间的转换关系是由点云和地图匹配确定（可以和i理解为运行重定位时机器人的位置在map中的位姿），camera_Init和body之间是激光雷达里程计

这里还有个/Odometry话题，反映的是雷达里程计（camera_Init->body），而非机器人在map中的位姿。

2、将地图转换为move_base的2D栅格地图

pcd2pgm下载链接

将PCD点云转换成pgm格式的2D栅格地图
保存地图到本地

bash 复制代码

rosrun map_server map_saver map:=/map -f mymap

3、将Mid360实时发布的3D点云转换成2D点

pointcloud_to_laserscan下载链接

这里我用的是ROS1

bash 复制代码

git clone -b lunar-devel https://github.com/ros-perception/pointcloud_to_laserscan.git

直接编译后完成