今天這篇內容收集了我在過去寫控制程式，以及為了寫文而準備示範程式的過程中，因應各種奇怪狀況而總結出來的解決方法，看起來會有一點東拼西湊的感覺，還請多包涵了。

專案調整

因為前面介紹啟動檔、世界檔和模型檔的範例在這裡不適用，因此要設定新的啟動檔、世界檔和調整模型的一些數值。

啟動檔設置

為了讓啟動檔的內容有更明確的分工，我將啟動檔（.launch 檔）改成巢狀結構，分成主要啟動檔（demo_main.launch）、負責啟動模擬環境的啟動檔（demo_world.launch）和負責生成機器人的啟動檔（put_robot_in_world.launch 和 spawn_sdf.launch）。

• demo_main.launch 這個啟動檔負責做為整個啟動檔的入口，分別匯入環境與模型的啟動檔。

  <?xml version="1.0" encoding="UTF-8" ?>
  <launch>
      <!-- 匯入啟動 Gazebo 模擬環境的 launch -->
      <include file="$(find my_simulation)/launch/demo_world.launch">
      </include>
  
      <!-- 匯入將無人機放入 Gazebo 的 launch -->
      <include file="$(find my_simulation)/launch/put_robot_in_world.launch" />
  </launch>
  

• demo_world.launch 內容主要包含設定 Gazebo 的環境參數（就不需要每次都重新指定路徑、插件和模型材質等），以及指定使用 demo_world.world 為模擬環境。

  <?xml version="1.0" encoding="UTF-8"?>
  <launch>
      <!--設定環境參數 gazebo path-->
      <env name="GAZEBO_MODEL_PATH" value="$GAZEBO_MODEL_PATH:$(find ardrone_gazebo)/models:$(find my_simulation)/models" />
      <env name="GAZEBO_PLUGIN_PATH" value="$GAZEBO_PLUGIN_PATH:$(find ardrone_gazebo)/plugins"/> 
      <env name="GAZEBO_RESOURCE_PATH" value="$GAZEBO_RESOURCE_PATH:$(find ardrone_gazebo)/meshes"/>
  
      <!-- 定義 Gazebo 標準 launch 要用的參數 -->
      <arg name="world_name" default="$(find my_simulation)/worlds/demo_world.world" />
      <arg name="debug" default="false" />
      <arg name="gui" default="true" />
      <arg name="paused" default="false" />
      <arg name="use_sim_time" default="true" />
      <arg name="headless" default="false" />
      <arg name="verbose" default="true" />
  
      <!-- 匯入 gazebo_ros 的標準 launch -->
      <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)" />
          <arg name="verbose" value="$(arg verbose)" />
      </include>
  </launch>
  

• put_robot_in_world.launch 是用來定義機器人的參數，以及匯入 spawn_sdf.launch 來動態將機器人載入模擬環境（比起從世界檔放有更多參數可以調整）。

  <?xml version="1.0" encoding="UTF-8" ?>
  <launch>
  
      <!-- 定義無人機模型的參數 -->
      <arg name="sdf_robot_file" value="$(find ardrone_gazebo)/models/ardrone_gazebo/ardrone_gazebo.sdf"/>
      <arg name="robot_name" value="ardrone_gazebo" />
  
      <!-- 定義命名空間 -->
      <group ns="ardrone">  
          <!-- 匯入生成機器人的 launch -->
          <include file="$(find my_simulation)/launch/spawn_sdf.launch">
              <arg name="sdf_robot_file" value="$(arg sdf_robot_file)"/>
              <arg name="robot_name" default="$(arg robot_name)"/>
  
              <arg name="x" value="0.0" />
              <arg name="y" value="0.0" />
              <arg name="z" value="0.0" />
              <arg name="roll" value="0.0"/>
              <arg name="pitch" value="0.0"/>
              <arg name="yaw" value="0.0" />
          </include>
      </group>
  </launch>
  

• spawn_sdf.launch 內容如下，它會設定 Gazebo 與 ROS 之間的通訊參數，和啟動 gazebo_ros 套件的節點將無人機放入。

  <?xml version="1.0" encoding="UTF-8"?>
  <!-- 將 SDF 檔案放入 gazebo -->
  <launch>
      <!-- arg 從外部傳入 -->
      <arg name="sdf_robot_file" default="" />
      <arg name="robot_name" default="" />
      <arg name="x" default="" />
      <arg name="y" default="" />
      <arg name="z" default="" />
      <arg name="roll" default=""/>
      <arg name="pitch" default=""/>
      <arg name="yaw" default="" />
  
      <!-- 設定 ROS 參數伺服器的 robot_description-->
      <param name="robot_description" command="$(find xacro)/xacro '$(find ardrone_gazebo)/urdf/ardrone_gazebo.urdf'" />
  
      <!-- 啟動 ROS 節點：發布機器人的狀態資訊 -->
      <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
          <param name="publish_frequency" type="double" value="50.0" />
          <param name="tf_prefix" value="$(arg robot_name)" />
      </node>
  
      <!-- 啟動 ROS 節點：生成模型, 使用 gazebo_ros 套件的 spawn_model 節點, 並且設定輸入參數 -->
      <node name="$(arg robot_name)_spawn_urdf" 
          pkg="gazebo_ros" type="spawn_model" 
          respawn="false" output="screen"
          args="-file $(arg sdf_robot_file) -sdf 
              -x $(arg x) -y $(arg y) -z $(arg z)  
              -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) 
              -model $(arg robot_name)"/>
  </launch>
  

世界檔設置

除了載入無人機模型之外，也在世界檔新載入一個球場的模擬環境，世界檔（demo_world.world）定義如下：

<?xml version="1.0"?>

<sdf version="1.5">
	<world name="default">
		<!-- 全域光源 -->
		<include>
			<uri>model://sun</uri>
		</include>

		<!-- 基礎地面 -->
		<include>
			<uri>model://ground_plane</uri>
		</include>

		<!-- 球場 -->
		<include>
			<uri>model://robocup_3d_field</uri>
			<pose>0 0 0 0 0 0</pose>
		</include>
    </world>
</sdf>

模型設置

如果直接用原始的無人機模型數值來飛，光是起飛就會自己亂轉亂飄跑到非常遠的地方，因此需要把模型的質量加重，慣性也加大一下。

在 ardrone_gazebo 目錄裡找到 ardrone_gazebo.sdf ，拉到中間的部分修改 <inertial> 標籤的內容。經過幾次的測試，我自己找到比較好的數值是 <mass> 改成 1.5， <ixx> 和 <iyy> 改 0.15，最後 <izz> 改成 0.3。

<!-- pose of the quadrotor in the world frame -->
<pose>0 0 0 0 0 0</pose>
<link name='base_link'>
<inertial>
    <mass>1.5</mass>
    <pose>0 0 0 0 0 0</pose>
    <inertia>
        <ixx>0.5</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.15</iyy>
        <iyz>0</iyz>
        <izz>0.3</izz>
    </inertia>
</inertial>

如果調整後還是覺得無人機在環境裡很容易失控亂飛，可以每次將數值以 10 % 的幅度增減來微調。

啟動模擬和控制程式

當全部設定完成後，就可以啟動模擬環境啦！

$ roslaunch my_simulation demo_main.launch

成功啟動之後，會看到 Gazebo 中出現綠色草皮球場和放在原點的無人機。這時在另一個終端機運行上一篇寫好的控制程式（記得轉換成可執行檔），就能看到無人機自己飛起來了。

$ rosrun my_simulation square_move.py

明明只是寫了各種設定文件，但不知不覺就讓無人機自己飛起來了呢（雖然飛得很歪）！那接下來要做什麼呢？

如果執行現在的控制程式 square_move.py，會發現它的功能只是根據「軸向」、「持續時間」和「速度或角速度」來執行每一個動作，因此要精準地控制無人機的位置還是頗有難度。

那要怎麼辦呢？請繼續看下一篇的內容吧。