用python规模化生成gazebo中 .world .png 和 .yaml文件

用python规模化生成gazebo中 .world  .png 和 .yaml文件

首先，我们需要知道gazebo中基本的 .world  文件组成

然后我们将.world文件中的模型与 .png中的灰度图像进行对应，同时在 .yaml文件中进行坐标、分辨率等参数的对应。

最后我们开始生成  .world  .png  .yaml文件

一、三种文件的格式

    1、.world文件（http://gazebosim.org/tutorials?cat=tools_utilities&tut=logging_playback）

  (...)
  
    (...)
  
  
    (...)
  
  
    (...)
  

    其中我们一般只需要修改 < model >标签就够了，以下是我的一个循环插入圆柱体作为障碍物的代码。分别是模型申明调用和模型定义，首先是申明，然后是模型的定义。

        1.cylinder_number是指圆柱体的个数，而  c_value_list['c_value_x_list'][i]  代表了第 i 个障碍物的 x 坐标，y同理。

        2.表示相对于某个frame的tf转换，这里为空，也就是默认使用基于源坐标系。

        3.标签的学习(http://wiki.ros.org/urdf/XML/link),第二个for循环是对模型的定义

        4. 是使用在当前.world文件中的模型是否进行缩放，分别对应 x y z 三个坐标方向

for i in range(cylinder_number):
        model_msg_0 =  " \n\
          " + str(c_value_list['c_value_x_list'][i]) + " "+ str(c_value_list['c_value_y_list'][i]) + " 0 0 -0 0\n\
          1 1 1\n\
          \n\
            " + str(c_value_list['c_value_x_list'][i]) + " "+ str(c_value_list['c_value_y_list'][i]) + " 0 0 -0 0\n\
            0 0 0 0 -0 0\n\
            0 0 0 0 -0 0\n\
            0 0 0 0 -0 0\n\
          \n\
        \n"
        msg_1 = msg_1 + model_msg_0

  

for i in range(cylinder_number):
        model_msg_1 = "\n\
          \n\
            0 0 0 0 -0 0\n\
            \n\
              0.284858\n\
              \n\
                0.0288096\n\
                0\n\
                0\n\
                0.0288096\n\
                0\n\
                0.010143\n\
              \n\
              0 0 0 0 -0 0\n\
            \n\
            0\n\
            0\n\
            1\n\
            \n\
              \n\
                \n\
                  "+str(model_radius)+"\n\
                  1\n\
                \n\
              \n\
              \n\
                \n\
                0.3 0.3 0.3 1\n\
                0.7 0.7 0.7 1\n\
                0.01 0.01 0.01 1\n\
                0 0 0 1\n\
                \n\
                  __default__\n\
                \n\
              \n\
              0 0 0 0 -0 0\n\
              1\n\
              0\n\
            \n\
            \n\
              0\n\
              10\n\
              0 0 0 0 -0 0\n\
              \n\
                \n\
                  "+str(model_radius)+"\n\
                  1\n\
                \n\
              \n\
              \n\
                \n\
                  \n\
                    1\n\
                    1\n\
                    0 0 0\n\
                    0\n\
                    0\n\
                  \n\
                  \n\
                    1\n\
                    0\n\
                    0\n\
                    1\n\
                    \n\
                      0\n\
                    \n\
                  \n\
                \n\
                \n\
                  0\n\
                  1e+06\n\
                \n\
                \n\
                  0\n\
                  1\n\
                  1\n\
                  \n\
                    0\n\
                    0.2\n\
                    1e+13\n\
                    1\n\
                    0.01\n\
                    0\n\
                  \n\
                  \n\
                    1\n\
                    -0.01\n\
                    0\n\
                    0.2\n\
                    1e+13\n\
                    1\n\
                  \n\
                \n\
              \n\
            \n\
          \n\
          1\n\
          1\n\
          " + str(c_value_list['c_value_x_list'][i]) + " "+ str(c_value_list['c_value_y_list'][i]) + " 0 0 -0 0\n\
        \n"
        msg_2 = msg_2 + model_msg_1
    # 结尾部分
    msg = msg_2 +"\n\
          \n\
            7.46544 4.75826 14.2026 4e-06 1.5658 1.56416\n\
            orbit\n\
            perspective\n\
          \n\
        \n\
      \n\
    \n\
    "

        将上面这两个 for 循环用起来，就能不断添加模型了，当然了，是相同的模型。在最结尾的部分获得一个 msg ，这里面包含了一个 .world文件的全部内容，将其写入文件即可。

    2、.png文件

       简单来说我们用到的就是像素点位置和值的信息。如果想了解可直接百度一下

    3、.yaml文件

        image：指向的文件（可以给一个具体路径），我自己实践过的支持的图片格式是 .png 和 .pgm格式。

        resolution：分辨率，表示 0.05m 是一个像素点，也就是 1m 切分为 20 个像素点

        origin：[x y z]，本图的坐标系相对于世界坐标系的位置

        nagate：默认是0

        occupied_thresh: 0.65 当像素占据的概率大于 0.65时候认为是完全占据的。
        free_thresh: 0.196 当像素占据的概率小于 0.196时候认为是完全空的。

image: ./world_cylinder_10_0.png 
resolution: 0.050000
origin: [0.000000, 0.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

二、.world文件模型与 .png中图形进行对应

    我们这里在 .world文件中建的模型是一些圆柱体以及一堵围墙，对应到 .png 中就是一些圆圈和一些一些直线的组合。

        1.因为图像其实就是一个二维数组，所以使用numpy创建一个矩阵(空白图），并将其初始化为一张白图

        2.然后使用opencv在图上画线，这些点的坐标、线条厚度都是可以与模型中的、x y z等联系起来，我这里偷懒了，直接用了固定的
                \n\等属性

            2.1 img：指定图片

            2.2 pt1，pt2：起始点和终点

            2.3 color：线条颜色，1是黑色

            2.4 thickness：线条厚度，-1表示实心圆，填充

        3.for循环添加圆圈作为障碍物，与直线不同的就是圆心坐标center（x,y）和半径radius ，

        由于opencv中图像的坐标是左上角为原点，向右为x轴正方向，向下为y轴正方向，

        而gazebo中使用的是向右为x轴正方向，向上为y轴正方向，

        也就是.world文件和.png文件的坐标系是关于x轴对称的，

emptyImage3 = np.zeros((map_shape, map_shape), np.uint8)
emptyImage3[...] = 255

cv2.line(img=emptyImage3, pt1=(1, 1), pt2=(1, 199), color=1, thickness=2)
cv2.line(img=emptyImage3, pt1=(1, 1), pt2=(199, 1), color=1, thickness=2)
cv2.line(img=emptyImage3, pt1=(199, 199), pt2=(1, 199), color=1, thickness=2)
cv2.line(img=emptyImage3, pt1=(199, 199), pt2=(199, 1), color=1, thickness=2)

for i in range(cylinder_number):
    cv2.circle(emptyImage3, (round(c_value_list['c_value_x_list'][i]/resolution), map_shape-(round(c_value_list['c_value_y_list'][i]/resolution))), radius=round(model_radius/resolution), color=1, thickness=2)

 

三、生成文件

    最后生成文件，emptyImage3在第二部分得到，yaml_file 是手动写入的一条信息，格式都一样，只是改一点点地方。至于最后一个写入 msg，就是前面第一部分得到的 .world 文件的全部信息。分别使用以下三种方式写入文件，大功告成！

    cv2.imwrite(save_dir + 'world_cylinder_' + str(cylinder_number) + '_'+ str(j) + '.png', emptyImage3)
    yaml_file = "image: ./world_cylinder_" + str(cylinder_number) + "_" + str(j) + ".png \nresolution: 0.050000\norigin: [0.000000, 0.000000, 0.000000]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"
    with open(save_dir + 'world_cylinder_' + str(cylinder_number) + '_' + str(j) + '.yaml', 'w') as f:
        f.write(yaml_file)
    with open( save_dir + 'world_cylinder_' + str(cylinder_number) + '_' + str(j) + '.world', 'w' ) as f:
        f.write(msg)

源码：https://download.csdn.net/download/zlb_zlb/12037502

下载设置错了，应该设置成没有积分限制的，没有积分的同学，可以评论一下给你发