Fork frome this repo and added positioning path display.
-
For details, please refer to blog
-
STEP0. UnZip Rosbag in file LaserDataset
-
STEP1. Download the repository to your ROS workspace：catkin_ws/src
-
STEP2. Make：'catkin_make -j8'
-
STEP3. Run: 'roslaunch particle_filter_localization pf_localization.launch' . Need to modify the Rosbag path.
rosrun rqt_graph rqt_graph
rosrun rqt_tf_tree rqt_tf_tree箭头分别表示从父节点到子节点的变换，所以这里注意在写变换矩阵的时候，是先平移，后旋转，并且是按照动轴的方式（矩阵右乘）定义的变换矩阵，这种方式的好处就是直观，可以想着怎么把父节点，一步一步地变换为子节点的姿态。
- 先旋转后平移为
${P}'=RP+T$ - 先平移后旋转为
${P}'=R(P+T)$
通常的坐标变换标准为：map-->odom-->base_link-->other_link(laser、cam)
- odom到base_link的变换是由里程计（轮式、编码器等传感器）获得；
- base_link到cam是camera在机器人中的安装位置（固定外参）；
- map到laser、cam的变换是由定位模块给出，例如，gmapping、SLAM-ORB2；
注意到图里存在环，也就是有两条路径都可以从map到cam，里程计（轮式、编码器等传感器）是存在漂移（轮子打滑：里程计没测量到）的，而里程计的计算是连续的，没有跳变，所以漂移量就转移到了map-->odom这个过程。即是odom-->base_link，map-->cam，base_link-->cam是确定的，那么误差就传递给map-->odom了！
所以，有时候会看到，刚开始的时候map和odom坐标系是重合的，跑着跑着map和odom就不重合了，因为这里把map到odom的变换定义为误差项了！
定位的意义就在于连通这个环，也就是测出map-->odom之间的变换，这样一来，bas_link（也就是机器人）在map中的位置就是已知的。如果只是里程计计算，由于漂移的存在，实际上计算出的位置也是不准的。
实际ROS项目中的里程计会发布两个Topic：
- /odom: 类型为nav_msgs/Odometry，反映里程计估测的机器人位置、方向、线速度、角速度信息。
- /tf: 主要是输出odom_frame和base_frame之间的tf。这段tf反映了机器人的位置和方向变换，数值与/odom中的相同。
- geometry_msgs/Transform定义的就是frame_id与child_frame_id之间的变换。
- frame_id ==> 父节点:odom坐标系
- child_frame_id ==> 子节点：base_link坐标系
- 摘录MOOC网ROS教程中Gmapping小节的一段话，MOOC.ROS.Gmapping
输入
-
/tf以及/tf_static： 坐标变换，类型为第一代的tf/tfMessage或第二代的tf2_msgs/TFMessage 其中一定得提供的有两个tf，一个是base_frame与laser_frame之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame与odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。
-
/scan :激光雷达数据，类型为sensor_msgs/LaserScan
/scan很好理解，Gmapping SLAM所必须的激光雷达数据，而/tf是一个比较容易忽视的细节。尽管/tf这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而slam_gmapping要从中读取的数据是base_frame与laser_frame之间的tf,只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是base_frame与odom_frame之间的tf，这个tf反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame和laser_frame之间。
因此slam_gmapping会从/tf中获得机器人里程计的数据。
输出
- /tf： 主要是输出map_frame和odom_frame之间的变换
- /slam_gmapping/entropy： std_msgs/Float64类型，反映了机器人位姿估计的分散程度
- /map： slam_gmapping建立的地图
- /map_metadata： 地图的相关信息
输出的/tf里又一个很重要的信息，就是map_frame和odom_frame之间的变换，这其实就是对机器人的定位。通过连通map_frame和odom_frame，这样map_frame与base_frame甚至与laser_frame都连通了。这样便实现了机器人在地图上的定位。
同时，输出的Topic里还有/map，在上一节我们介绍了地图的类型，在SLAM场景中，地图是作为SLAM的结果被不断地更新和发布。
在这个demo中，map就是取的odom的原点。
path前期容易跳变，可以在刚开始的时候采用里程计发布，滤波稳定后再使用滤波后的path进行显示。 The 'path' is easy to jump in the early stage. It can be released by the odometer at the beginning, and the filtered path can be used for display after the filtering is stabilized.
最后一张图片中的箭头和自己的习惯不一样，其他是一样的，具体体现在Localizer::measurementUpdate()函数求激光点在世界坐标系下的坐标：
The arrow in the last picture in the dataset 'readme document' is different from my representation habit, and the others are the same, which is specifically reflected in the Localizer::measurementUpdate() function to compute the coordinates of the laser point in the world coordinate system: