11.如何在非固定大小的地图图片上正确渲染ROS小车的位置和运行轨迹

最近需要实现一个功能：在使用cartographer建图时，需要实时显示AGV的位置，并且由用户控制是否显示AGV的运动轨迹

基本思路就是：获取里程计坐标系和车体坐标系的坐标变换，然后通过该坐标变换计算出小车相对于建图原点的位姿，该位姿包含了一组x,y的坐标，然后记录该组坐标并通过这三个坐标将该位姿转换为QT图片中的像素坐标，然后根据像素坐标使用QT进行渲染

但是遇到这样的一个问题：使用cartographer建图时，图片并不使固定大小的，而是会随着地图的增大。显示地图的图片逐渐增大的过程中，地图中的轨迹就会显示错误。如下图

gif中红色的点表示小车运动的轨迹，当地图变大时，图片也会变大，图片变大后导致对应的轨迹点就会出现偏移，不能很好的表示小车的运动轨迹

 

原因：主要是渲染时，轨迹的存储出现了问题

path_.push_back(QPointF( currobotpose_.x / mapresolution_ + originx_,
      currobotpose_.y / mapresolution_ + originy_));

for (auto p:path_) {
    painter.drawPoint(p);
}

部分代码如上，根据当前的位姿计算出一个QPointF，然后将该QPointF的临时对象放入一个容器中，然后通过QT中的painter进行绘制。但是，问题在于，容器中的点都是固定点，并不会随着地图的增大而发生对应的改变，当地图变大时，点的X坐标和Y坐标都是不变的，但是地图的长和宽变大，导致定时渲染时的坐标位置发生跳变。

举个例子：比如图像中的一点的坐标是（10,10），图片的大小是100*100，当图片变成200*200时，坐标就会重新在200*200中的（10,10）位置绘制，导致点沿着左上角偏移

 

解决办法：

经过一番思考，发现该其实小车轨迹点的绘制是根据小车的位姿currobotpose_来绘制的，，将一系列的实时位姿存储起来，这样渲染时算出来的点就是实时的点，而不是固定的点。同时，为了防止容器中的元素过多，导致内存不够，会记录上一时刻的位姿，当上一时刻的位姿和当前位姿不同时，才记录位姿（防止小车停在同一个位置不动，但是位姿还在不停的添加，这样会导致内存占用过多）

修改后，效果如下

可以发现，小车的轨迹点不会出现条变得情况

之所以问题能解决，是因为容器中存储的位姿是实时的，所以点也是实时计算的，而不是固定的点