判断amcl位置与地图匹配程度

      navigation启动时，amcl为机器人新建并维护一个粒子滤波器，通过匹配分值判断当前机器人位置与地图的匹配程度。为amcl添加一个输出匹配分值的功能。为了滤除噪声，取1s内5个分值（0.2s间隔）作为该时间段的分值。新建线程，每秒统计一次分值量化判断机器人位置的丢失程度。

      取出当前时刻分值（pf.c）。

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>

#include "amcl/pf/pf.h"
#include "amcl/pf/pf_pdf.h"
#include "amcl/pf/pf_kdtree.h"

// match_score declare by cabin avoiding error of multiple definition
double match_score_;

...

void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data){
...
  // Compute the sample weights
  total = (*sensor_fn) (sensor_data, set);
 //get score by cabin
  match_score_ = total; 
... 
}

...

// Delive the score by cabin
double get_match_score(){
	return match_score_;
}

      统计1s时间段内分值（amcl_node.cpp）。

...
class AmclNode
{
...
    std::thread *computeCurrentScoreThread;
    void computeScoreThread();
}

...

AmclNode::AmclNode() :
        sent_first_transform_(false),
        latest_tf_valid_(false),
        map_(NULL),
        pf_(NULL),
        resample_count_(0),
        odom_(NULL),
        laser_(NULL),
	private_nh_("~"),
        initial_pose_hyp_(NULL),
        first_map_received_(false),
        first_reconfigure_call_(true)
{
...
   computeCurrentScoreThread = new std::thread(&AmclNode::computeScoreThread,this);
}

...

//=================compute the matching score by cabin============================
void AmclNode::computeScoreThread(){
	double sum_score;
	double sub_score;
	int count;
	while(1){
		sub_score = get_match_score();
		sum_score += sub_score;
		count++;
		sleep(0.2);
		if(count>5){
			std::cout<<"========"<<"pf filter score:"<<sum_score<<"======"<<&std::endl;
			count =0;
			sum_score = 0.0;
			}
		}
}

      则在navigation运行时，每秒都会输出机器人当前位置与地图的匹配度作为参考。