ROS中階筆記（四）：機器人模擬—Gazebo物理模擬環境搭建（重點）
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