本文想用a-loam算法实现建图定位导航。命令流程如下：
1.开启16线激光雷达
roslaunch lslidar_c16_decoder lslidar_c16.launch
2.启动aloam
roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
- 启动laser-scan-match提供odom
roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
4.启动amcl定位
roslaunch aloam_velodyne amclbak.launch
在rviz中add map，添加先验二维栅格地图。
在map坐标系中，点击2D pose estimate，开始定位。
仅仅是跑通了。首先，odom是很不准的，只是为了要这个话题，其次更重要的是，amcl定的位不是机器人走的位置（我移动雷达amcl_pose是没反应的，有相应反应的是aft_mapped，绿箭头点哪里amcl_pose就到哪里。）坐标系之间的关系没有理清，amcl的参数也没弄懂，所以，下面，一点点来弄懂。
理思路顺序：
- A-LOAM的原理简要分析；
- A-LOAM中各种坐标系、tf、话题的意义；
- laser-scan-match提供的odom是否对多线激光雷达也适用；
- amcl的原理，tf的关系，配置文件中参数的意义。
一、A-LOAM原理分析
camera_init：这个坐标系相当于world，是初始相机拍到的那个位置，是不动的。
aft_mapped：这个是laser的位置坐标系，随着雷达的移动而移动。
这是我的节点图：
scanRegistration.cpp:首先接收激光雷达话题：/velodyne_points，发布laserCloud（按线堆栈的全部点云）：
/velodyne_cloud_2，发布各种角点和面点：/laser_cloud_sharp（角点）/laser_cloud_less_sharp（降采样角点）/laser_cloud_flat（面点）/laser_cloud_less_flat（降采样面点）
发布去除点：/laser_remove_points（其实没有发布）
laserOdometry.cpp：主要发布了这个里程计信息：/laser_odom_to_init（ 在/laser_odom坐标系下的）
laserMapping.cpp:订阅角点面点，里程计，全部点云，发布各种点云地图：/laser_cloud_surround（周围五帧点云集合（降采样后的））、/laser_cloud_map（总点云地图）、/velodyne_cloud_registered（全部注册的点云）、构图处理后的当前世界坐标系下的雷达位姿：****/aft_mapped_to_init(topic 是有的也有内容，但是不知道为啥在节点图里没显示)可以用这个来作为odom使用 感觉比scan-matcher会准的多。
二、 laser-scan-match
暂时就用这个odom
三、AMCL原理及tf、topic
关于原理的关键词：蒙特卡洛定位 自适应 粒子滤波 具体不在多说
订阅的话题：
- scan (sensor_msgs/LaserScan)
- tf (tf/tfMessage)
- initialpose (geometry_msgs/PoseWithCovarianceStamped)
- map (nav_msgs/OccupancyGrid)
主要是要将scan（激光）、tf和map主题提供给amcl。
这里面最重要的就是这个tf！！！
他究竟要什么样的tf呢，谁和谁转换的？看下面的图：
需要将base_link，odom，map三个frame连接起来。下面是正确的tf tree：
而我的tf tree，tf比较多但是也不算混乱。
发布的话题：
- amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
- particlecloud (geometry_msgs/PoseArray)
- tf (tf/tfMessage)
Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map.
突然发现 我的/odom话题发出来了却无人订阅
而 amcl订阅了啥tf我也不清楚，我想知道这些节点都订阅了哪些话题
查看节点图，我的老天鹅。。感觉amcl会吃所有的tf???是不是应该精简一下tf。
tf修剪中-----------------------------------
我把amcl_node.cpp的base_link换成了base_footprint：
private_nh_.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
别忘了再catkin_make哦
精简完（也不知道对不对）amcl启动的时候竟然报错：Couldn’t transform from laser_link to base_link, even though the message notifier is in use
现在根本没有base_link啊！？
咋回事呢，最终在amcl.cfg文件里找到了base_link，改成base_footprint再试试！还是不行！
解决了！在launch文件里加了
<param name="base_frame_id" value="base_footprint"/>
就可以了。
现在的问题是，amcl_pose不随着雷达动，只有当我用绿箭头点一下地图，大家一起动！
NOW 看代码 追根溯源
// amcl_node.cpp 这些下划线让我想起了这是我之前有改动的地方，这个“map”很奇怪哦
//
geometry_msgs::PoseStamped amcl_pose;
amcl_pose.header.stamp = ros::Time::now();
amcl_pose.header.frame_id = "map";
amcl_pose.pose.position.x=p.pose.pose.position.x;
amcl_pose.pose.position.y=p.pose.pose.position.y;
amcl_pose.pose.position.z=0;
pub_amcl_pose.publish(amcl_pose);
amcl_path.poses.push_back(amcl_pose);
pub_amcl_path.publish(amcl_path);
amcl_path.header.stamp = ros::Time::now();
amcl_path.header.frame_id = "map";
///
重新下载了源码发现这段真的是我自己加的。。。。。。。大乌鱼
替换以后再试一下：成功了。
继续搞move_base
参考了别人的机器人urdf模型，四个参数配置文件，move_base的launch文件。
这些文件我没有深入看，目前只是跑通了，但是实验效果不好，感觉和odom的精度有很大关系。
坏了 amcl_pose又不动了！！！！！！！昨晚还好好的呢！！
用单线雷达跑了gmapping+amcl+move_base是可以的。
所以感觉问题就出在aloam他不能边建图边导航，lego-loam可以边建图边导航，之后可能要试一下。还有就是里程计，想把代码移植到车辆底盘中（用底盘带的odom）
现在 想了解一下amcl的参数配置
// amcl的launch文件
<?xml version="1.0"?>
<launch>
<!-- 启动地图服务，导入先验地图 -->
<node pkg="map_server" type="map_server" name="map_server" args="/home/mxy/map.yaml"/>
<!-- 车体坐标系和雷达坐标系之间的tf转换 有0.45米的高度差-->
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_init"
args="0.0 0.0 0.45 0.0 0.0 0.0 /base_footprint /camera_init 100" />
<!-- 启动amcl node -->
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<remap from="scan" to="scan"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="use_map_topic" value="true"/> //订阅地图话题
<param name="odom_model_type" value="omni"/> //车辆模型选择
<param name="odom_alpha5" value="0.1"/> //平移相关的噪声参数，仅在omni模型中使用
<param name="transform_tolerance" value="0.5" /> //发布tf变换的时间
<param name="gui_publish_rate" value="10.0"/> //可视化时，发布信息的最大速率（-1.0表示禁用）
<param name="laser_max_beams" value="300"/> //更新过滤器时要在每次扫描中使用多少光束
<param name="min_particles" value="500"/> //允许的最少粒子数
<param name="max_particles" value="5000"/> //允许的最大粒子数
<param name="kld_err" value="0.1"/> //真实分布与估计分布的最大误差
<param name="kld_z" value="0.99"/> //（1-p）的上标准正常分位数，其中P是估计分布误差小于kld_err的概率
<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="laser_z_hit" value="0.9"/> //模型z_hit部分的混合参数
<param name="laser_z_short" value="0.05"/>//模型z_short部分的混合参数
<param name="laser_z_max" value="0.05"/>//模型z_max部分的混合参数
<param name="laser_z_rand" value="0.5"/>//模型z_rand部分的混合参数
<param name="laser_sigma_hit" value="0.2"/>//模型z_hit部分中使用的高斯模型的标准偏差
<param name="laser_lambda_short" value="0.1"/>//模型z_short部分的指数衰减参数
<param name="laser_model_type" value="likelihood_field"/> //模型的选择
<!-- <param name="laser_model_type" value="beam"/> --> //模型也有beam
<param name="laser_min_range" value="1"/> //最小扫描范围
<param name="laser_max_range" value="5"/> //最大扫描范围
<param name="laser_likelihood_max_dist" value="2.0"/>//地图上测量障碍物膨胀的最大距离
<param name="update_min_d" value="0.1"/> //执行一次滤波器更新所需要的平移距离
<param name="update_min_a" value="0.5"/> //执行一次滤波器更新所需要的旋转角度（rad）
<param name="resample_interval" value="1"/> //重采样之前滤波器的更新次数
<param name="transform_tolerance" value="0.1"/> //发布tf变化的时间，以指示此变换在未来有效
<param name="recovery_alpha_slow" value="0.0"/> //慢速平均权重滤波器的指数衰减率，用于决定和是通过添加随机姿态进行恢复操作，0.0表禁用
<param name="recovery_alpha_fast" value="0.0"/> //快速平均权重滤波器的指数衰减率，用于决定和是通过添加随机姿态进行恢复操作，0.0表禁用
<param name="base_frame_id" value="base_footprint"/> //机器人底盘坐标系
</node>
</launch>