文件amcl_node.cpp
最主要的几个函数是构造函数,handleMapMessage, laserReceived。尤其是最后一个
杂项 sigintHandler
回调函数在关闭程序之前只有一句amcl_node_ptr->savePoseToServer();
,也就是关闭前把最近一次的机器人位姿存入参数服务器,这就可以理解为什么关掉程序后,用rosparam get
获得的initial_pose_x
等位姿仍然是以前的。
amcl
根据输入参数来决定运行方式。从代码中可以看到amcl支持数据回放的,运行时只需要通过参数--run-from-bag
指定bag文件即可
从main函数可以看出,amcl的所有业务逻辑都是由AmclNode类的构造函数完成
AmclNode构造函数 通过参数服务器,根据launch文件里的大量参数,对各类变量赋值
成员函数updatePoseFromServer
,该函数就是用来从参数服务器上获取机器人的初始位姿和位姿误差的协方差矩阵的。
发布amcl_pose
和particlecloud
两个主题,分别用于输出机器人位姿估计和粒子集合。
接着注册三个服务:global_localization
用于获取机器人的全局定位,request_nomotion_update
则用于手动的触发粒子更新并发布新的位姿估计,set_map
用于设定机器人位姿和地图信息。 这几个平时都用不到
接下来构建激光传感器的消息过滤器对象和tf2的过滤器,并注册回调函数laserReceived。1 2 3 4 5 6 7 8 laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100 ); laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>( *laser_scan_sub_, *tf_, odom_frame_id_, 100 ); laser_scan_filter_->registerCallback (boost::bind (&AmclNode::laserReceived, this , _1));
这里的message_filter为ROS系统提供了一些通用的消息过滤方法, 它对接收到的消息进行缓存,只有当满足过滤条件后才输出,在需要消息同步的时候应用比较多。这里主要是同时监听激光扫描消息和里程计坐标变换,同步两者的输出。 这一套路在GMapping中也有用到。
订阅用于初始化机器人位姿估计的主题initialpose
根据运行参数use_map_topic_
获取地图,或者订阅地图主题,或者通过requestMap
请求static_map
服务。因为一般取false,所以后面我们直接看requestMap
函数,也就是如下逻辑: 定义m_force_update
为false,后面会用到
最后定义一个计时器用于每隔15s检查一次激光雷达的接收数据,如果期间没有收到新的数据给出告警
requestMap 和 handleMapMessage requestMap
函数请求static_map
服务成功后,调用函数handleMapMessage
处理接收到的地图数据,进一步完成AmclNode的初始化工作,内容庞大的其实是handleMapMessage
handleMapMessage
的流程图在这里
释放了与地图相关的内存和对象之后,通过函数convertMap将地图消息转换成amcl中的地图数据结构。
接着构建粒子滤波器对象,并完成初始化:1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 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_; 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 ] = 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
构建里程计对象,根据launch文件里的参数配置里程计模型
构建雷达传感器对象,根据运行参数laser_model_type_
构建不同模型的雷达
最后考虑到,在接收到地图数据之前,有可能已经接收到了机器人的初始位姿,这里调用函数applyInitialPose处理
pf_alloc 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 pf_t *pf_alloc (int min_samples, int max_samples, double alpha_slow, double alpha_fast, pf_init_model_fn_t random_pose_fn, void *random_pose_data) { int i, j; pf_t *pf; pf_sample_set_t *set; pf_sample_t *sample; srand48 (time (NULL )); pf = calloc (1 , sizeof (pf_t )); pf->random_pose_fn = random_pose_fn; pf->random_pose_data = random_pose_data; pf->min_samples = min_samples; pf->max_samples = max_samples; pf->pop_err = 0.01 ; pf->pop_z = 3 ; pf->dist_threshold = 0.5 ; pf->current_set = 0 ; for (j = 0 ; j < 2 ; j++) { set = pf->sets + j; set->sample_count = max_samples; set->samples = calloc (max_samples, sizeof (pf_sample_t )); for (i = 0 ; i < set->sample_count; i++) { sample = set->samples + i; sample->pose.v[0 ] = 0.0 ; sample->pose.v[1 ] = 0.0 ; sample->pose.v[2 ] = 0.0 ; sample->weight = 1.0 / max_samples; } set->kdtree = pf_kdtree_alloc (3 * max_samples); set->cluster_count = 0 ; set->cluster_max_count = max_samples; set->clusters = calloc (set->cluster_max_count, sizeof (pf_cluster_t )); set->mean = pf_vector_zero (); set->cov = pf_matrix_zero (); } pf->w_slow = 0.0 ; pf->w_fast = 0.0 ; pf->alpha_slow = alpha_slow; pf->alpha_fast = alpha_fast; pf_init_converged (pf); return pf; }
pf_init 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 void pf_init (pf_t *pf, pf_vector_t mean, pf_matrix_t cov) { int i; pf_sample_set_t *set; pf_sample_t *sample; pf_pdf_gaussian_t *pdf; set = pf->sets + pf->current_set; pf_kdtree_clear (set->kdtree); set->sample_count = pf->max_samples; pdf = pf_pdf_gaussian_alloc (mean, cov); for (i = 0 ; i < set->sample_count; i++) { sample = set->samples + i; sample->weight = 1.0 / pf->max_samples; sample->pose = pf_pdf_gaussian_sample (pdf); pf_kdtree_insert (set->kdtree, sample->pose, sample->weight); } pf->w_slow = pf->w_fast = 0.0 ; pf_pdf_gaussian_free (pdf); pf_cluster_stats (pf, set); pf_init_converged (pf); return ; }