Gmapping 源码阅读(3)————初始化地图

1. 创建第一个地图

在初始化地图时，首先需要创建第一个地图。Gmapping使用OccupancyGrid类来表示地图，OccupancyGrid类定义了一个二维网格，每个网格单元包含一个概率值，表示该单元格被占据的可能性。

void Gmapping::initMapper(const nav_msgs::OccupancyGrid::ConstPtr& grid_map)
{
  if (!got_first_scan) {
    if (!grid_map) {
      ROS_ERROR("GMapping初始化错误：没有输入地图!");
      return;
    }
    initMaps(grid_map->info.resolution, grid_map->info.width, grid_map->info.height, grid_map->info.origin.position.x, grid_map->info.origin.position.y,
             grid_map->data);
  }
}

在initMapper()函数中，如果got_first_scan为false，表示还没有收到第一帧的激光数据，此时需要检查grid_map是否为null。如果grid_map为null，则表示没有输入地图，此时会输出错误信息并返回。

如果grid_map不为null，则会调用initMaps()函数来创建第一个地图。initMaps()函数接收分辨率、宽度、高度、原点位置等参数，并使用这些参数来初始化地图。

2. 初始化激光雷达位姿

在创建第一个地图后，需要初始化激光雷达位姿。Gmapping使用tf_listener来获取激光雷达相对于基座的位姿。

if (tf_listener_.waitForTransform(map_frame_, laser_frame_, ros::Time(0), ros::Duration(0.05))) {
  geometry_msgs::TransformStamped transform;
  tf_listener_.lookupTransform(map_frame_, laser_frame_, ros::Time(0), transform);

  double yaw = tf::getYaw(transform.transform.rotation);
  laser_pose = lcl::Pose2D(transform.transform.translation.x, transform.transform.translation.y, yaw);
}

在initMapper()函数中，使用tf_listener_.waitForTransform()函数来检查是否能够获取激光雷达相对于基座的位姿。如果能够获取，则调用tf_listener_.lookupTransform()函数来获取位姿。

获取位姿后，将位姿中的旋转部分提取出来，并将其转换为yaw角。最后，将激光雷达位姿存储在laser_pose变量中。

3. 粒子滤波器初始化

在初始化地图和激光雷达位姿后，需要初始化粒子滤波器。Gmapping使用pf_utils::ParticleFilter类来实现粒子滤波器。

// 初始化粒子滤波器
pf_ = boost::shared_ptr<pf_utils::ParticleFilter<Particle> >(new pf_utils::ParticleFilter<Particle>);

// 设置粒子数
pf_->setParticleCount(initial_particles);

在initMapper()函数中，使用boost::shared_ptr来创建一个粒子滤波器对象。然后，设置粒子数，并使用setParticleCount()函数来设置粒子数。

4. 阅读地图初始化代码时需要注意的细节

在阅读地图初始化代码时，需要注意以下细节：

• Gmapping使用OccupancyGrid类来表示地图，OccupancyGrid类定义了一个二维网格，每个网格单元包含一个概率值，表示该单元格被占据的可能性。
• Gmapping使用tf_listener来获取激光雷达相对于基座的位姿。
• Gmapping使用pf_utils::ParticleFilter类来实现粒子滤波器。

希望本文对您有所帮助。如果您有任何问题或建议，欢迎在评论区留言。