ESP32 micro-ROS搭载RPLIDAR N10+MPU6050，SLAM Toolbox无地图输出

阿华AIGC实验室

2026-6-13

手持建图系统SLAM无地图输出问题解决方案

问题背景

搭建的手持建图系统包含ESP32（micro-ROS）、RPLIDAR N10、MPU6050，PC端使用ROS 2 Humble，通过micro_ros_agent连接，配合imu_filter_madgwick、robot_localization EKF和slam_toolbox实现建图。目前RViz中无法显示地图，切换到laser帧能看到扫描数据但无地图生成。

核心问题排查与修复步骤

1. 修复时间戳同步问题

ESP32代码中自定义的stamp_now函数未与ROS系统时间同步，仅使用自增计数器，导致SLAM工具无法正确关联时序数据。

修改方法：
替换stamp_now函数，使用micro-ROS提供的系统时间接口：

static inline void stamp_now(builtin_interfaces__msg__Time &t)
{
  rcl_clock_t clock;
  RCCHECK(rcl_clock_get_default(&clock));
  RCCHECK(rcl_clock_get_now(&clock, &t));
}

同时删除原函数中的静态变量nano_count。

2. 修正LaserScan消息字段错误

原代码中angle_min计算逻辑错误，且缺少scan_time和time_increment字段，导致SLAM工具无法正确解析激光数据：

修改ESP32中LaserScan发布部分的代码：

/* when we wrapped ≈360°, publish ------------------------- */
if (p_idx>=450) {
  // 修正角度范围为0到2π（360°）
  scan.angle_min = 0.0f;
  scan.angle_max = 2 * M_PI;
  scan.angle_increment = 0.8f * DEG_TO_RAD;
  // 设置扫描时间间隔（根据N10的扫描频率，假设10Hz，即0.1s）
  scan.scan_time = 0.1f;
  scan.time_increment = scan.scan_time / p_idx;
  
  memcpy(scan.ranges.data, ranges, p_idx*sizeof(float));
  scan.ranges.size = p_idx;

  stamp_now(scan.header.stamp);
  RCSOFTCHECK( rcl_publish(&laser_pub, &scan, NULL) );
  p_idx = 0;
}

注：如果N10实际扫描频率不同，调整scan.scan_time为对应的周期（如20Hz则设为0.05f）。

3. 完善SLAM Toolbox参数配置

原参数文件缺少关键的里程计输入配置，SLAM需要EKF输出的里程计数据来进行位姿估计：

更新slam_params.yaml：

solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
mode: mapping
map_frame: map
odom_frame: odom
base_frame: base_link
scan_topic: /scan
odom_topic: /odometry/filtered
use_odometry: true
range_data_count: 450
min_laser_range: 0.05
max_laser_range: 12.0
throttle_scans: 1
use_sim_time: false
use_scan_matching: true
use_scan_barycenter: true
publish_tf: true
transform_timeout: 0.5

4. 验证EKF配置与输出

确保ekf_config.yaml正确订阅IMU数据并输出里程计话题：

检查ekf_config.yaml中的核心配置：

ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true  # 手持建图通常为2D模式
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    imu0: /imu/data
    imu0_config: [true, true, true,
                  true, true, true,
                  true, true, true]

启动后通过ros2 topic echo /odometry/filtered验证里程计数据是否正常输出。

5. RViz配置检查

确保Map插件订阅的话题为/map（slam_toolbox默认发布话题）
确认Fixed Frame设置为map时，TF树完整（map→odom→base_link→laser），可通过ros2 run tf2_tools view_frames.py再次验证。

验证步骤

重新编译ESP32代码并上传
启动launch文件：ros2 launch your_package slam_with_imu.launch.py
用ros2 topic list确认所有话题（/scan、/imu/data_raw、/imu/data、/odometry/filtered、/map）均存在
移动手持设备，观察RViz中是否生成地图

内容的提问来源于stack exchange，提问作者michael lai