ROS中階筆記（四）：機器人仿真—Gazebo物理仿真環境搭建（重點）
1 ros_control
- ROS為開發者提供的機器人控制中間件
- 包含一系列控制器接口、傳動裝置接口、硬件接口、控制器工具箱等等
- 可以幫助機器人應用功能包快速落地，提高開發效率
Controller_manager 與很多controller都已經很完善了，需要寫的就是robothw部分
ros_control的功能主要通過controller來實現的
1.1 ros_control安裝
ros_control需要apt-get命令安裝一下，否則這里的launch文件運行不起來，可以通過下列命令安裝：
$ sudo apt-get install ros-kinetic-gazebo-ros-control
2 Gazebo仿真步驟（重點）
- 配置機器人模型
- 創建仿真環境
- 開始仿真
2.1 配置物理仿真模型
在~/catkin_ws/src/mbot_description/urdf/xacro/gazebo下，找到mbot_base_gazebo.xacro文件
2.1.1 第一步：為link添加慣性參數和碰撞屬性
之前我們創建xacro文件的時候，每個物體只有一個簡單的可視化屬性，如果要在Gazebo中實現的話，需要對這些可視化模型的每個link添加碰撞屬性以及慣性參數。
以base_link為例：
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
對比一下前面的base_link，會發現其下面多了一個collision以及cylinder_inertial_matrix，這里需要對每一個link都添加這些屬性。關於慣性矩陣的運算，可以自行百度。這里也通過宏定義的形式給出了一個慣性矩陣的計算方式：
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
上面一個為球體的慣性矩陣計算公式，下面為柱體的計算公式。然后在后面通過調用這些宏來實現慣性矩陣的計算。
同樣的，需要給所有的link添加慣性參數和碰撞屬性。
2.1.2 第二步：為link添加gazebo標簽
標簽的主要內容是為了配置每個link在gazebo中的顏色（gazebo與rivz顏色設置不兼容）例如：
給base_link設置了藍色：
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
2.1.3 第三步：為joint添加傳動裝置
由於gazebo的控制是針對於joint做控制的；
例如：我有一個真實的機器人，如果我想讓它的輪子動起來，那么我需要一個電機驅動這個輪子做運動。這一步的內容就相當於給我們的機器人模型添加一個電機，也就是一個傳動裝置：
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${prefix}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
2.1.4 第四步：添加gazebo控制器插件（相當於驅動板）
我們現在相當於有了一個模型，通過第三步我們模型有了電機，那我們要讓這個電機轉起來我們還需要一個控制器，我們給控制器發送一個目標速度這個控制器會發送具體轉速給電機，所以在我們的模型文件我們添加一個控制器：
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so"> <!-- gazebo提供得差速控制器插件 -->
<!-- 控制器所需參數 -->
<rosDebugLevel>Debug</rosDebugLevel>
<publishWheelTF>true</publishWheelTF>
<robotNamespace>/</robotNamespace><!-- 機器人命名空間 訂閱和發布得話題 前面 會加上命名空間 /說明沒有添加-->
<publishTf>1</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>left_wheel_joint</leftJoint> <!-- 控制得joint在哪里，必須和上面得joint名稱一致 -->
<rightJoint>right_wheel_joint</rightJoint><!-- 控制得joint在哪里，必須和上面得joint名稱一致 -->
<wheelSeparation>${wheel_joint_y*2}</wheelSeparation><!-- 兩個輪子得間距 -->
<wheelDiameter>${2*wheel_radius}</wheelDiameter>
<broadcastTF>1</broadcastTF>
<wheelTorque>30</wheelTorque>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic> <!-- 訂閱得話題：速度控制指令 -->
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic> <!-- 發布里程計信息 -->
<robotBaseFrame>base_footprint</robotBaseFrame><!-- 設置controler所控制的機器人的坐標系是哪個坐標系 -->
</plugin>
</gazebo>
注意：我加入注釋是為了更好的理解；xacro中最好不要出現中文，否則可能會報參數問題導致無法正常啟動。
在gazebo中加載機器人模型
在~/catkin_ws/src/mbot_gazebo/launch/下創建view_mbot_gazebo_empty_world.launch文件
<launch>
<!-- 設置launch文件的參數 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 運行gazebo仿真環境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加載機器人模型描述參數 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_gazebo.xacro'" />
<!-- 運行joint_state_publisher節點，發布機器人的關節狀態 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 運行robot_state_publisher節點，發布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<!-- 在gazebo中加載機器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
$ roslaunch mbot_gazebo view_mbot_gazebo_empty_world.launch
# 在空環境中可看到機器人模型
建議：為保證模型順利加載，請提前將模型文件庫下載並放置到 ~/.gazebo/models 下
在home文件夾下，ctrl + h 可以查找到所有的隱藏文件
下載地址：
https://bitbucket.org/osrf/gazebo_models/downloads/
2.2 創建仿真環境
當機器人出現在gazebo的時候，可以看到我們加載的文件里面只有一個機器人，那我們希望能給它添加一些障礙物，這時應該怎么做呢？
2.2.1 方法一：直接添加環境模型
①插入模型
模型放置到~/.gazebo/models 文件夾下——在gazebo的左側列表點擊“insert”（可以看到里面有很多的模型，我們只需要從列表中拖出我們需要的模型放置到仿真環境中就可以）
②保存仿真環境
File——Save World As——放置在功能包~/catkin_ws/src/mbot_gazebo/worlds下面（路徑自己選擇，主要是在 ~/catkin_ws/src/）
③關閉gazebo界面
如果這些模型不能滿足建模需求也可以通過三維設計軟件搭建模型放到文件夾中使用。例如solidworks建模
2.2.2 方法二：使用Building Editor
①創建模型
Editor——Building Editor，上面界面用於圖形編輯，下面是仿真環境；比如說繪制三維場景中的一堵牆或者一個門。
File——Save保存我們的模型文件（自己設置模型文件名字）——Exit Building Editor（退出編輯界面），可以看到我們的仿真環境已經在gazebo中顯示；
②保存仿真環境
File——Save World As——放置在功能包~/catkin_ws/src/mbot_gazebo/worlds下面（路徑自己選擇，主要是在 ~/catkin_ws/src/）
③關閉gazebo界面
2.2.3 方法二與方法一相結合
2.3 開始仿真
在~/catkin_ws/src/mbot_gazebo/launch下 view_mbot_gazebo_room.launch
如果啟動gazebo時希望加載這個模型，可以在launch文件最前面添加：
<!-- 設置launch文件的參數 -->
<arg name="world_name" value="$(find mbot_gazebo)/worlds/room.world"/>
<!-- 運行gazebo仿真環境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
$ roslaunch mbot_gazebo view_mbot_gazebo_room.launch # 啟動仿真環境
下面大家可以進行一些仿真功能的實現
我們希望它能動起來，那么打開一個鍵盤控制節點：
$ roslaunch mbot_teleop mbot_teleop.launch
同時也可以通過監聽odom數據觀察機器人的位置。
$ rostopic echo /odom
orientation: # 四元數描述機器人的姿態
x:-2.51798726555e-05
y:-7.59817327643e-06
Z:-0.954841297132
w:0.297116301471
3 傳感器仿真
只有機器人的話，並不能做具體的實驗，我們希望它能與環境交互，那么我們需要給機器人添加傳感器。
類似於之前的控制器插件，gazebo同樣擁有很多的傳感器插件。
在~/catkin_ws/src/mbot_description/urdf/xacro/sensors/下
3.1 攝像頭仿真
在~/catkin_ws/src/mbot_description/urdf/xacro/sensors/下camera_gazebo.xacro文件
需要一個攝像頭的xacro文件，它需要添加碰撞屬性以及慣性屬性外，還需要添加攝像頭傳感器插件，具體代碼如下：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<xacro:macro name="usb_camera" params="prefix:=camera">
<!-- Create laser reference frame -->
<link name="${prefix}_link">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</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>
</xacro:macro>
</robot>
啟動這個帶攝像頭的模型的launch文件（只是改變機器人模型文件和仿真環境文件路徑即可）：
$ roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch
<launch>
<!-- 設置launch文件的參數 -->
<arg name="world_name" value="$(find mbot_gazebo)/worlds/playground.world"/><!-- 改這個就行 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 運行gazebo仿真環境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加載機器人模型描述參數 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_with_camera_gazebo.xacro'" /> <!-- 改這 -->
<!-- 運行joint_state_publisher節點，發布機器人的關節狀態 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 運行robot_state_publisher節點，發布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<!-- 在gazebo中加載機器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
$ rqt_image_view # 查看攝像頭仿真圖像
再啟動一個鍵盤控制節點：
$ roslaunch mbot_teleop mbot_teleop.launch
移動機器人的位置我們會發現圖像也是會隨之改變。
3.2 RGB-D攝像頭仿真（kinect）
加載另外一個launch文件：
$ roslaunch mbot_gazebo view_mbot_with_kinect_gazebo.launch
啟動rviz界面查看攝像頭的效果：
$ rosrun rviz rviz
添加一個pointcloud2，topic選擇/kinect/depth/points。fixed frame選擇Kinect_link就可以看到相應的點雲信息了。
同樣也可以選擇添加一個image顯示圖像信息。
3.3 激光雷達仿真
啟動激光雷達launch文件：
$ roslaunch mbot_gazebo view_mbot_with_laser_gazebo.launch
同樣可以啟動rviz可視化界面查看參數：
$ rosrun rviz rviz
#點擊Add一個robotmodel，fixed frame選擇odom，可以顯示一個機器人模型。
#再添加一個laserscan,topic選擇scan。就可以在rviz中顯示出激光信息了。size設置激光點大小。
4 問題解決
4.1 Gazebo閃退及無法運行（虛擬機中）
在官網下載models文件放在./gazebo文件夾下出現錯誤，Gazebo閃退及無法運行，提示問題
VMware: vmw_ioctl_command error 無效的參數.
Aborted (core dumped)
[gazebo_gui-3] process has died [pid 3469, exit code 134, cmd /opt/ros/kinetic/lib/gazebo_ros/gzclient __name:=gazebo_gui __log:=/home/ggk/.ros/log/29a04806-93bd-11ea-950a-000c298133c0/gazebo_gui-3.log].
log file: /home/ggk/.ros/log/29a04806-93bd-11ea-950a-000c298133c0/gazebo_gui-3*.log
解決方法：
第一步，關閉虛擬機的3D圖形加速：
虛擬機設置—顯示器—關閉3D圖形加速；
原因：因為虛擬機開啟3D圖形加速，圖形內存不夠用，加載不出來。
第二步，關閉硬件加速：
echo "export SVGA_VGPU10=0" >> ~/.bashrc
source ~/.bashrc
詳細解決方法：https://blog.csdn.net/wangguchao/article/details/88777162
5 參考資料
ros_control http://www.guyuehome.com/890
http://wiki.ros.org/ros_control
rviz與Gazebo的區別 https://www.zhihu.com/question/268658280/answer/340190866
Gazebo仿真教程 http://www.gazebosim.org/tutorials
使用gazebo中的插件 http://www.guyuehome.com/388