沉默杀手HomeArchivesTagsCategories
源码分析(二) handleMapMessage 函数
|激光SLAMamcl和粒子滤波|
Word count: 718|Reading time: 3 min

调用关系.png
实际用的是requstMap这一支

准备工作

参数是const nav_msgs::OccupancyGrid& msg

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
// 地图的宽高，分辨率
ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
         msg.info.width,
         msg.info.height,
         msg.info.resolution);
// 如果目前的全局坐标系不是map
if(msg.header.frame_id != global_frame_id_)
  ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
           msg.header.frame_id.c_str(),
           global_frame_id_.c_str()  );

//free相应的指针
freeMapDependentMemory();
// Clear queued laser objects because they hold pointers to the existing map
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();

处理获得的地图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// AMCL所用的地图类型
typedef struct
{
  // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  double scale;  // Map scale (m/cell)

  // Map dimensions (number of cells)
  int size_x, size_y;
  
  // The map data, stored as a grid
  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
} map_t;

转换为AMCL的地图信息

1
2
3
4
5
6
7
8
9
//转换成AMCL的标准地图，0->-1(不是障碍);100->+1（障碍）;else->0（不明）
map_ = convertMap(msg);  // 类型map_t

//将不是障碍的点的坐标保存下来, 构成地图栅格的空闲区域
free_space_indices.resize(0);
for(int i = 0; i < map_->size_x; i++)
  for(int j = 0; j < map_->size_y; j++)
    if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
      free_space_indices.push_back(std::make_pair(i,j));

从参数服务器获取初始位姿及方差放到pf中

1
updatePoseFromServer();

其实就是rosparam把6个参数放到init_pose_和init_cov_两个数组中，供下面进一步赋值

创建粒子滤波器并初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
pf_ = pf_alloc(min_particles_, max_particles_,
               alpha_slow_, alpha_fast_,
               (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
               (void *)map_);
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;

pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];

pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;

pf_alloc和pf_init是两个重要函数。 pf_alloc只在这里调用了一次，pf_init在这里和applyInitialPose里调用

专门分析pf_alloc

专门分析pf_init

定义里程计与激光雷达并初始化数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
// 设置里程计参数
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );

// Laser
delete laser_;
// max_beams_就是参数laser_max_beams
// map_ 是AMCL所用的地图格式, 在上面获得
laser_ = new AMCLLaser(max_beams_, map_);
ROS_ASSERT(laser_);
// 4个形参为似然域模型的参数
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                  laser_likelihood_max_dist_);

SetModelLikelihoodField函数就是使用前三个形参用于赋值, 然后调用 map_update_cspace(this->map, laser_likelihood_max_dist_);， 这个是似然域模型中最复杂的函数
它的专门分析在这里

参数laser_likelihood_max_dist默认为2，是地图上做障碍物膨胀的最大距离，用于似然域模型

这几个参数没有用在似然域模型: laser_z_short, laser_z_max, laser_lambda_short, laser_likelihood_max_dist

applyInitialPose()

// In case the initial pose message arrived before the first map,
// try to apply the initial pose now that the map has arrived.
可能初始位姿的信息比第一个map先到达，map到达后apply初始位姿

1
2
3
4
5
6
7
8
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
if( initial_pose_hyp_ != NULL && map_ != NULL ) {
  pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
  pf_init_ = false;

  delete initial_pose_hyp_;
  initial_pose_hyp_ = NULL;
}