宇树机器狗go2仿真添加2d雷达、单目相机和深度相机

文章来源：https://www.guyuehome.com/detail?id=1892031577741905922

0.前言

上一篇文章教大家如何搭建宇树机器狗go2的仿真环境（宇树机器狗go2 gazebo仿真环境）而这次我们就来教大家如何在gazebo仿真环境当中添加雷达、深度相机、单目相机等传感器(有简易版和精细版)。宇树机器狗go2仿真会作为一个长期项目来进行不定期更新，仿真不只限于在gazebo上实现，后面还会在isaac sim等仿真平台上教大家如何搭建，其目的还是帮助各位都能以较低的成本加入到宇树机器狗go2的学习中。gazebo的仿真对电脑的需求会低于isaac sim等仿真平台，这对实验条件要求会低很多，更很适合学校小实验室或个人进行仿真实验。

1.URDF文件和XACRO文件

1.URDF文件

1.URDF简介

URDF(统一机器人模型描述文件格式)是ROS中的重要机器人模型描述格式，ROS提供了URDF文件的c++解析器，可以解析URDF文件中使用XML格式的机器人模型。
ROS wiki - urdf
在ros的官方维基百科中可以看到对urdf文件的介绍和使用教程，偶尔使用urdf文件的时候是需要查看官方urdf文档的。

2.URDF命令工具

·作用：检查、梳理模型文件。
1.安装liburdfdom-tools

sudo apt install liburdfdom-tools

2.check_urdf命令
·作用:解析URDF文件，并显示解析过程中发现的问题

check_urdf src/unitree_ros/robots/go2_description/urdf/go2_description.urdf

执行上面的命令后可以观察到每一个关节的父子连接关系。

3.urdf_to_graphiz命令
·作用:查看URDF模型的整体结构。

urdf_to_graphiz src/unitree_ros/robots/go2_description/urdf/go2_description.urdf

执行上面的命令后会在当前终端路径下生成URDF模型整体结构图

打开生成的pdf文件就可以看到机器人的URDF模型结构关系图。

2.XACRO文件

1.XACRO简介

URDF文件不具备代码复用的特性，机器人会拥有大量相似的关节组件或者是传感器，使用URDF文件会使得机器人模型文件变得相当难读，xacro文件可以简化机器人模型文件的描述。同时xacro文件件也可以更好在gazebo中的添加传感器插件:
Gazebo-Docs: Sensors

2.XACRO命令工具

URDF命令工具都可以在XACRO中使用。

2.宇树机器狗go2的gazebo仿真模型

1.宇树机器狗go2模型文件

1.urdf文件下的宇树机器狗go2

宇树机器狗go2的urdf机器人描述文件robot.xacro在工作空间的src/unitree_ros/robots/go2_description/urdf/目录下

cd go2_ws/src/unitree_ros/robots/go2_description/urdf/

roslaunch go2_description go2_rviz.launch

在终端中输入上面的命令会展示使用go2_description.urdf文件的机器狗go2，查看结构图会发现urdf文件中的机器狗go2配备了雷达的link和joint。

2.宇树机器狗go2的xacro文件

宇树机器狗go2的机器人描述文件robot.xacro在工作空间的src/unitree_ros/robots/go2_description/xacro/目录下

cd go2_ws/src/unitree_ros/robots/go2_description/xacro/

宇树机器狗go2的xacro文件当中是没有加入有关radar雷达的link和joint，但imu和前置相机front_camera的link和joint都配备好了。

2.宇树机器狗go2的gazebo仿真描述文件

cd go2_ws/src/unitree_ros/robots/go2_description/xacro/

在gazebo.xacro文件中描述了宇树机器狗go2的一些关节物理描述，和相关传感器插件和描述。

3.添加简易的传感器模型

1.添加简易雷达模型

打开robot.xacro，将下面的简易版雷达描述加入到robot描述中，这里的雷达是外接的雷达，使用的是一个圆柱体进行简易描述。

<!--配置雷达简易版，使用圆柱体表示-->
<link name="lidar"><visual><geometry><cylinder radius="0.03" length="0.06" /></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><cylinder radius="0.03" length="0.03" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.1" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="lidar_joint" type="fixed"><parent link="base" /><child link="lidar" /><origin xyz="0.23 0 0.10377" /><axis xyz="0 0 1" /></joint>
-->

将简易版雷达加入后效果如下图圆柱体：

2.添加简易深度相机模型

打开robot.xacro，将下面的简易版深度相机描述加入到robot描述中，这里的深度相机是外接的深度相机，使用的是一个长方体进行简易描述。

<!--简易版深度相机，用长方体进行表示-->
    <link name="real_sense"><visual><geometry><box size="0.01 0.1 0.02"/></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material></visual><collision><geometry><box size="0.01 0.1 0.02"/></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="0.05" /><inertia ixx="6.64E-04" ixy="0" ixz="0" iyy="6.64E-04" iyz="0" izz="1.02E-03" /></inertial></link><joint name="realsense_joint" type="fixed"><parent link="base" /><child link="real_sense" /><origin xyz="0.29 0 0.093" /><axis xyz="0 0 1" /></joint>

将简易版深度相机加入后效果如下图长方体：

3.整体效果

将简易版雷达和简易版深度相机加入后会获得如下图一样的效果：

4.添加精细传感器

1.添加精细雷达模型

1.获取模型文件

下载下面的雷达模型文件：雷达模型文件
将下载好的雷达模型文件放到机器狗go2描述文件的meshes文件夹下：

2.更改robot.xacro文件

打开robot.xacro，将下面的精细版雷达描述加入到robot描述中，这里的雷达是外接的雷达，使用的是一个精细模型进行描述。

<!--模型好一点的2d激光雷达-->
<linkname="lidar"><inertial><originxyz="-0.0120919804823413 0.0023160815953297 -0.0191850779635995"rpy="0 0 0" /><massvalue="0.0792224121739075" /><inertiaixx="2.10579541640659E-05"ixy="-1.189626176555E-07"ixz="4.23690141563745E-07"iyy="2.51190032657276E-05"iyz="-7.86831091483644E-09"izz="4.38241306407012E-05" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://go2_description/meshes/lidar.STL" /></geometry><materialname=""><colorrgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://go2_description/meshes/lidar.STL" /></geometry></collision></link><jointname="lidar_joint"type="fixed"><originxyz="0.23 0 0.123"rpy="0 0 0" /><parentlink="base" /><childlink="lidar" /><axisxyz="0 0 0" /></joint>

将精细版雷达加入后效果如下图雷达：

2.添加精细深度相机模型

1.获取模型文件

下载下面的深度相机模型文件：深度相机模型
将下载好的深度相机模型文件放到机器狗go2描述文件的meshes文件夹下：

2.更改robot.xacro文件

打开robot.xacro，将下面的精细版深度相机描述加入到robot描述中，这里的深度相机是外接的深度相机，使用的是一个精细模型进行描述。

<!--模型好一点的深度相机-->
 <linkname="real_sense"><inertial><originxyz="-0.00975078931607951 0.00306064913353049 -7.04374991291334E-05"rpy="0 0 0" /><massvalue="0.103311749598955" /><inertiaixx="7.35323632954531E-05"ixy="3.0437488683846E-06"ixz="2.08538534427428E-08"iyy="7.40427481540393E-06"iyz="-1.93018760531168E-08"izz="7.2832884533889E-05" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://go2_description/meshes/real_sense.STL" /></geometry><materialname=""><colorrgba="0.666666666666667 0.698039215686274 0.768627450980392 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://go2_description/meshes/real_sense.STL" /></geometry></collision></link><jointname="real_sense_joint"type="fixed"><originxyz="0.29 0 0.093"rpy="0 0 0" /><parentlink="base" /><childlink="real_sense" /><axisxyz="0 0 0" /></joint>

将精细版深度相机加入后效果如下图深度相机：

5.添加传感器插件

1.gazebo仿真相关插件和物理描述文件

打开gazebo.xacro文件，文件里描述了宇树机器狗go2在gazebo仿真里面的仿真物理描述以及传感器插件（用于实现雷达、单目相机、深度相机等传感器的仿真表现）。

2.添加2d雷达传感器插件

打开gazebo.xacro文件，将下方2d雷达插件添加到文件中：

<!-- hokuyo 2d雷达插件-->
  <gazebo reference="lidar"><material>Gazebo/Grey</material><sensor type="ray" name="hokuyo_sensor"><pose>0 0 0.0124 0 0 0</pose><visualize>false</visualize><update_rate>40</update_rate><ray><scan><horizontal><samples>1081</samples><resolution>1</resolution><min_angle>-2.3561944902</min_angle><max_angle>2.3561944902</max_angle></horizontal></scan><range><min>0.1</min><max>10.0</max><resolution>0.01</resolution></range><noise><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="gazebo_ros_hokuyo_controller" filename="libgazebo_ros_laser.so"><topicName>/scan</topicName><frameName>lidar</frameName></plugin></sensor></gazebo>

3.添加深度相机传感器插件

这里深度摄像头的插件用kinect的插件代替。打开gazebo.xacro文件，将下方深度相机(kinect)插件添加到文件中：

<!-- real_sense  深度相机(kinect)插件 -->
<gazebo reference="real_sense"><sensor type="depth" name="real_sense"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>${60.0*3.14/180.0}</horizontal_fov><image><format>R8G8B8</format><width>640</width><height>480</height><https://bbs-object.s3.cn-north-1.jdcloud-oss.com><clip><near>0.05</near><far>8.0</far></clip></camera><plugin name="kinect_real_sense_controller" filename="libgazebo_ros_openni_kinect.so"><cameraName>real_sense</cameraName><alwaysOn>true</alwaysOn><updateRate>10</updateRate><imageTopicName>rgbhttps://bbs-object.s3.cn-north-1.jdcloud-oss.com_raw<https://bbs-object.s3.cn-north-1.jdcloud-oss.comTopicName><depthImageTopicName>depthhttps://bbs-object.s3.cn-north-1.jdcloud-oss.com_raw</depthImageTopicName><pointCloudTopicName>depth/points</pointCloudTopicName><cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName><depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName><frameName>real_sense</frameName><baseline>0.1</baseline><distortion_k1>0.0</distortion_k1><distortion_k2>0.0</distortion_k2><distortion_k3>0.0</distortion_k3><distortion_t1>0.0</distortion_t1><distortion_t2>0.0</distortion_t2><pointCloudCutoff>0.4</pointCloudCutoff></plugin></sensor></gazebo>

4.添加单目相机传感器插件

打开gazebo.xacro文件，将下方单目相机(kinect)插件添加到文件中：

 <!-- camera 单目相机插件 -->
  <gazebo reference="front_camera"><material>Gazebo/Grey</material><sensor type="camera" name="camera"><update_rate>30.0</update_rate><camera name="head"><horizontal_fov>1.3962634</horizontal_fov><image><width>800</width><height>800</height><format>R8G8B8</format><https://bbs-object.s3.cn-north-1.jdcloud-oss.com><clip><near>0.02</near><far>300</far></clip><noise><type>gaussian</type><!-- Noise is sampled independently per pixel on each frame.
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1]. --><mean>0.0</mean><stddev>0.007</stddev></noise></camera><plugin name="camera_controller" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><updateRate>0.0</updateRate><cameraName>camera</cameraName><imageTopicName>image_raw<https://bbs-object.s3.cn-north-1.jdcloud-oss.comTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>front_camera</frameName><hackBaseline>0.07</hackBaseline><distortionK1>0.0</distortionK1><distortionK2>0.0</distortionK2><distortionK3>0.0</distortionK3><distortionT1>0.0</distortionT1><distortionT2>0.0</distortionT2></plugin></sensor></gazebo>

5.整体效果

6.疑惑

奇怪的是我在使用check_urdf查看宇树机器狗go2的URDF关节图，以及使用urdf_to_graphiz生成的URDF结构模型图里面都看到了有关radar雷达的关节描述，但在机器狗go2的xacro文件中却没有，官方没有在xacro文件当中添加相关描述。我后面再将urdf文件中有关radar的描述迁移到xacro文件中并加入一个3d激光雷达进行匹配。

7.后记

下一次更新应该会解决掉将urdf文件中雷达link和joint描述迁移到xacro文件的问题，将现在的2d雷达更换成3d雷达用来跑fastlio等slam算法（应该是会换成Velodyne的3d雷达，mid360雷达的gazebo库听说是有失真现象，但后续应该也会加入到更新中），同时也会教一下查看机器狗go2的传动文件，感谢一位朋友提供了一些不错的建议。