使用MATLAB绘制Gazebo中的仿真小车的运动轨迹

   虽然，Gazebo中自带了绘图工具，当需要绘制一些简单的图像时，非常的方便，但是当需要绘制复杂的图像时，还是MATLAB更加合适一些。
   当我们仅仅使用MATLAB以可视化图像的形式去观察Gazebo中小车的运动轨迹，并不参与小车的控制时，完全没有必要以栅格地图的形式去复现gazebo中的仿真环境，当环境较复杂或需要多变的时候往往需要很大的工作量，本文通过图片的形式，将gazebo中仿真环境
   本文介绍一种通过图片的形式，将gazebo中仿真环境快速复现到MATLAB中，并在此基础上叠加显示仿真小车运动轨迹的方法

  一、计算两幅地图间的变换比例

   （1）首先确定gazebo中仿真环境的长度及宽度，先通过工具栏中的工具，将视野调至俯视图

   依次选中gazebo的仿真环境的上下左右边界，并在左侧的pose栏中查看其坐标值，比如本文所用的例子中上边界为的X值为9.925000，下边界为-9.925，即在gazebo的仿真环境中，其在X轴上长度为19.85m。左边界的Y值为10.1235，右边界的Y值为-10.2265，即仿真环境在Y轴上长度为20.35

   （2）将俯视图下的仿真环境，截图并保存，作为MATLAB中的地图使用，截图前，可在view菜单栏，取消Origin坐标轴的显示

   本例中的截图如下所示

   在属性中可查看其像素值，也就是在MATLAB中以图片显示的地图的长度和宽度分别为666和656

   （3）现在拿到了两幅地图的长度和宽度，但是在计算两幅地图的比例变换系数前，还要注意一个特别特别重要的问题，坐标系的变换问题，在MATLAB读取照片的时候，其坐标系的原点在左上角，而不是左下角，如下所示：

   也就是说我们得到的gazebo中X轴长度应该对应MALTAB中的Y轴的长度，gazebo中Y轴长度应该对应MALTAB中的X轴的长度，因此我们得到MATLAB中地图与gazebo中地图的比例为 X轴（以MATLAB为准）=666/20.35=32.72727，Y轴（以MATLAB为准）=656/19.85=33.04786
   到这里我们就得到了两幅地图间的变换比例，也就是说在gazebo中小车沿着gazebo的的坐标系的X轴正方向移动1m，对应在MATLAB的地图中就是沿着Y轴的负方向移动了33.04786个像素点，同理，gazebo中小车沿着gazebo的的坐标系的Y轴正方向移动1m，对应在MATLAB的地图中就是沿着X轴的负方向移动了32.72727个像素点

  二、调试获取两幅地图间的原点偏移量

   （1）首先我们在MATLAB的命令行窗口，通过imread函数读取我们刚才保存的截图，最好是jpg格式，在MATLAB的命令行中png等格式也可以，但是在simulink的MATLAB Function模块中只允许使用jpg格式，如下图所示：

   刚才的照片我保存为gazebomap.jpg，在MALTAB的命令行输入以下语句将其读取，imread是读取图像，im2gray是将其转换成灰度图像（不转也可以），并命名为Image1

     Image1=im2gray(imread('gazebomap.jpg'));

   在工作空间使用imshow函数将其显示出来，如下图所示：

    imshow(Image1);

   （2）建立MATLAB与ROS的通讯，并通过subscribe模块订阅gazebo中发布的小车的位置信息，本部分内容在前面的文章中已经介绍过了，这里不再缀叙
   （3）使用MATLAB Function模块来编写程序，在刚才MALTAB显示的图片基础上绘制小车的实时位置，其代码如下：

function Gazebo_Track_Plotting(xr,yr,r,X_factor,Y_factor,X_Offset,Y_Offset)
hold on
%计算偏移量
rectangle('Position',[(-yr*X_factor+X_Offset)-r,(-xr*Y_factor+Y_Offset)-r,2*r,2*r],'Curvature',[1,1],'LineWidth',3,'FaceColor','y','EdgeColor','r'),axis equal
end

   该函数的输入参数中xr，yr是订阅的小车的位置信息（是在gazebo的坐标系下的），X_factor和Y_factor是我们在第一部分计算出的两幅地图之间的变换比例关系，通过constant模块输入给该函数，即X_factor=32.72727，Y_factor=33.04786，X_Offset和Y_Offset就是本步我们要调试的两幅地图下原点的偏移量，由于本例中在gazebo地图中原点差不多在其地图的中点出，而在MATLAB中其原点在左上角，因此X_Offset和Y_Offset大概为图片像素值的一半，即X_Offset应该在666/2=333附近，Y_Offset应该在656/2=328附近，因此将这两个值作为初始值赋给该函数

   由于matlab绘制单个单独的点不易观察，这里采用rectangle函数以该点为圆心绘制一个小圆的形式来代替了该点，r是该圆的半径

   （4）在gazebo中选中仿真小车，在左侧将其X和Y坐标均设为0，这时，gazebo中小车就会移动至gazebo中地图的零点，如下所示：

   （5）启动Simulink的仿真，观察此时根据大概的偏移量映射出的小车位置，如下所示：

   可以发现其相对gazebo中小车的位置有点偏左和偏下，因此应该适当减少Y轴的偏移量，同时适当增加X轴的偏移量，调节传送给Gazebo_Track_Plotting函数的变量X_Offset和Y_Offset，再次观察红点的位置，如此循环直至红点显示的位置为gazebo中原点的位置，经过调节本例中的X_Offset应该改为341，Y_Offset应该改为324，如下图所示

   到这里偏移量也就是找到了，同时，两个地图间的映射关系也就找到了

  三、观察效果

   控制gazebo中小车运动，观察MATLAB绘制的轨迹，如下所示：

   虽然由于通讯的原因，轨迹的绘制有一定的延时性，但是大体上满足了预期的设想

  本文介绍的内容到这里就结束了，在探索过程中遇到了一个奇怪的问题，在Simulink的MATLAB Function模块中，我曾经成功使用过imread（）函数，但是过了一天后同样的内容（或者说同样的文件），再也用不了了，只要在该模块里使用了imread函数，就会报以下错误（点击或拖动查看大图），一直没解决，期望得到大佬的指导