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

<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142 
117.5s -->
<launch>
    <param name="/use_sim_time" value="true"/>
    <node name="rosbag" pkg="rosbag" type="play" 
        args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/> <!--数据包自行修改替换-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>

    <node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
      <remap from="scan" to="base_scan" />
      <param name="transform_tolerance" value="0.2" />
      <param name="gui_publish_rate" value="5.0"/>
      <param name="save_pose_rate" value="0.5"/>
      <param name="laser_max_beams" value="100"/>
	  <param name="laser_max_range" value="10.0"/>
      <param name="min_particles" value="300"/>
      <param name="max_particles" value="2000"/>
      <param name="kld_err" value="0.05"/>
      <param name="kld_z" value="0.99"/>
      <param name="odom_model_type" value="diff-corrected"/>
      <param name="odom_alpha1" value="0.1"/>
      <param name="odom_alpha2" value="0.1"/>
      <!-- translation std dev, m -->
      <param name="odom_alpha3" value="0.1"/>
      <param name="odom_alpha4" value="0.1"/>
      <param name="odom_alpha5" value="0.0"/>
      <param name="laser_z_hit" value="0.90"/>
      <param name="laser_z_short" value="0.1"/>
      <param name="laser_z_max" value="0.05"/>
      <param name="laser_z_rand" value="0.05"/>
      <param name="laser_sigma_hit" value="0.2"/>
      <param name="laser_lambda_short" value="0.1"/>
      <param name="laser_lambda_short" value="0.1"/>
      <!-- <param name="laser_model_type" value="likelihood_field"/> -->
	  <param name="laser_model_type" value="likelihood_field_prob"/>
      <!-- <param name="laser_model_type" value="beam"/> -->
      
	  <param name="do_beamskip" value="true"/>
      <param name="beam_skip_distance"  value="0.25" />
      <param name="beam_skip_threshold" value="0.5" />  <!--0.3-->
      <param name="beam_skip_error_threshold_" value="0.8" />  <!--0.9-->
	  <param name="laser_likelihood_max_dist" value="2.0"/>
      <param name="update_min_d" value="0.2"/>
      <param name="update_min_a" value="0.3"/>
      <param name="odom_frame_id" value="odom"/>
      <param name="resample_interval" value="3"/>
      <param name="transform_tolerance" value="2.0"/>
	  
      <param name="recovery_alpha_slow" value="0.0"/>
      <param name="recovery_alpha_fast" value="0.0"/>
      <param name="initial_pose_x" value="47.443"/>
      <param name="initial_pose_y" value="21.421"/>
      <param name="initial_pose_a" value="-1.003"/>
	  <param name="initial_cov_xx" value="0.01"/>   <!--0.01-->   <!--default:0.5 * 0.5-->
	  <param name="initial_cov_yy" value="0.01"/>   <!--0.01-->   <!--default:0.5 * 0.5-->
	  <param name="initial_cov_aa" value="0.01"/> <!--0.01-->   <!--default:0.0685=(M_PI/12.0) * (M_PI/12.0) -->
	</node>
  <test time-limit="180" test-name="basic_localization_stage" pkg="amcl" 
        type="basic_localization.py" args="0 47.060 21.603 -1.053 0.75 0.75 90.0"/>
	<node pkg="rviz" name="rviz" type="rviz" args="-d $(find amcl)/rviz/rviz.rviz" />
</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计算资源，在撒粒子的时候可以提出这些无用的粒子，如下图，粒子会不会落在未探明区域 ，集中落在非障碍区域