源码分析(三)laserReceived回调函数

这是amcl_node.cpp中最重要的函数，蒙特卡洛算法都在它里面，我们前面分析几个算法时已经涉及到了它的几个部分，下面再串起来看一遍

laserReceived 回调函数

Amcl的业务逻辑总体就是在一个四五百行的巨大函数laserReceived中实现的，它就是刚刚我们提到的消息过滤器laser_scan_filter_的回调函数。

用变量lastlaser_received_ts记录下当前的系统时间，它用于判定是否长时间未接收到雷达数据。此外如果没有地图对象，将直接退出
AmclNode借助一些vector和map的容器，来支持多传感器。它使用lasers记录下当前构建的雷达对象，lasers_update标记雷达的更新状态，并通过一个string到int的map建立其雷达坐标ID到雷达对象在lasers_中的对应关系。
通过在frameto_laser中查找雷达的坐标ID，如果之前没有收到该雷达的消息，将新建一个对象保存在lasers中，并相应的在lasers_update中添加对应更新状态，建立映射关系。否则，我们就直接通过frameto_laser获取雷达对象的索引。
利用tf求出雷达坐标系的原点在base_link下的坐标，然后调用SetLaserPose设置雷达的位姿，angle暂时为0
getOdomPose获取收到雷达数据时的里程计位姿
if(pf_init_)的内容一定执行，如果里程计的数据显示机器人已经发生了明显的位移或者旋转，就标记所有的雷达更新标记为true
根据刚刚更新标识，我们调用里程计对象的UpdateAction接口完成运动更新，也就是机器人移动后执行else if(pf_init_ && lasers_update_[laser_index])的内容
机器人移动，执行if(lasers_update_[laser_index])的内容，根据激光的扫描数据更新滤波器。首先构建AMCLLaserData对象，并指定传感器对象和量程，然后将接收到的传感器数据拷贝到ldata对象中，最后通过激光传感器对象的UpdateSensor接口完成粒子滤波器的测量更新。这里发布了particlecloud话题

机器人移动，则执行if(resampled || force_publication)的内容，发布了amcl_pose话题

AmclNode::getOdomPose维护odom—>base_link的变换

map—>odom的tf变换在1410行左右：

ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_);
//  构造一个StampedTransform 需要tf::Transform ,stamp_,father_frame,child_frame 参数
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),// odom 到map的转换矩阵
                                    transform_expiration,
                                    global_frame_id_, odom_frame_id_);

this->tfb_->sendTransform(tmp_tf_stamped);//发布 odom 到map的转换矩阵转换