ROS下amcl跑test使用数据包bag下载地址大全

通过navigation包集下载的amcl/test/目录下缺少默认bag。致使test不能正常运行起来

需要自行下载http://download.ros.org/data/amcl/

必须下载 willow-full.pgm   拷到amcl/test目录下

基本定位下载basic_localization_stage_indexed.bag  拷到amcl/test目录下

全局定位下载global_localization_stage_indexed.bag 拷到amcl/test目录下

1.建立基本定位basic_localization_stage.launch

2.运行

终端 启动basic_localization_stage.launch

3.全局定位 均匀的撒粒子

从上图中 我们发现有很多粒子落子障碍物的边沿上，这里粒子是无效的，浪费CPU计算资源 ，优化思路，可以先对原始地图

经过膨胀之后或者把默认订阅 /map主题 改成订阅global/costmap(膨胀后的代价地图)的主题；然后抠出free space 区域存放到队列中free_space_indices

// Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)// 0
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));

4.指定局部区域中搜索定位

代码实现在一定区域内均匀撒粒子(指定局部区域中搜索定位)

在amcl中添加一个spread_particle 服务 在需要局部扩大定位时调用该服务 或者代码内部按照一定策略或者定时调用

 void AmclNode::handleSpreadParticlePose(void)
 {
 updatePoseFromServer();
  //指定定位区域的中心坐标
  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] = 2;
  pf_init_pose_cov.m[1][1] = 2;
  pf_init_pose_cov.m[2][2] = 1;

  delete initial_pose_hyp_;
  initial_pose_hyp_ = new amcl_hyp_t();
  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
  applyInitialPose();
  }

执行效果

从上图看很多粒子落在了障碍物和未知区域上，这些没用的粒子占用很多CPU计算资源，在撒粒子的时候可以提出这些无用的粒子，如下图，粒子会不会落在未探明区域 ，集中落在非障碍区域