模擬實驗

雙帆船的帆機通過繩索與兩個帆連線，帆的實際角度會受到風的影響，在0度和帆機輸出到角度之間。在模擬過程中假定帆的實際角度就是帆機輸出的角度。

1. 啟動圖形化介面

開啟一個新終端，執行以下命令，啟動第一個 ROS 節點。

roslaunch sailboat_launch start_tf_tree_onboat.launch

如果沒有報錯，Ctrl+Alt+T 開啟新終端，執行以下命令，啟動第二個節點。

roslaunch spare_function spare_function_simulation.launch

如果沒有報錯，Ctrl+Alt+T 開啟新終端，進入 simulation 資料夾下。此目錄需要根據自己安裝的目錄做修改，可能是這樣：

cd ~/sailboat_ws/src/Sailboat-Ros/Sh/simulation

也可能是這樣：

cd ~/sailboat_ws/src/2019-sailboat-ros/Sailboat-Ros/Sh/simulation

命令 cd 代表進入某目錄，緊接著 ~/ 表示「home/你的使用者名稱」這個目錄。總之，目錄需要自己根據實際狀況修改。也可以使用滑鼠和圖形化介面手動開啟 simulation 資料夾，然後滑鼠右鍵選擇開啟新終端。

進入 simulation 資料夾後，執行資料夾中的 interface.sh 指令碼檔案。命令如下：

./interface.sh

等待幾秒鐘後，將會陸續啟動 rviz 和 rqt 兩個應用程式。左邊的工作列中可以看到 rviz 的文字圖示和九個藍點的 rqt 圖示。
rviz
在 rviz 中點選相應的選項可以顯示帆船的路徑。

rqt
在 rqt 中勾選相應的核取方塊並展開對應的話題，可以觀察帆船當前的輸入和輸出。

2. 執行 python 檔案

最後需要啟動自己為帆船寫的程式，用於輸出帆和舵的角度。

cd ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts
python example.py

同樣，目錄需要根據自己 python 檔案的位置決定。上述命令中的 example.py 是範例程式碼，輸出的帆和舵的角度始終為0度。自己編寫帆船程式後需要使用 python 命令執行自己的 python 檔案。如果在執行 python 檔案時有報錯，請根據提示檢查自己寫的 python 檔案對應行存在的問題。

3. 偵錯

如果你的 python 檔案沒有報錯，說明程式在語法上通過了編譯，接下來需要檢查在邏輯上是否存在問題，帆船是否可以按照預期行駛。這時需要藉助 rviz 觀察帆船的路徑，通過 rqt 觀察輸出的資料。通過調整動態引數檢查是否所有情況下都可以正常執行。

如何自定義 rqt 中顯示的資料，以及如何調整動態引數（或者說，動態引數是什麼），將在下一章詳解。

python 檔案與 rqt 的關係

python 檔案的位置：

~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts

1. 獲取感測器資料

設計帆和舵的控制策略時，需要感測器資料。檔案在開頭匯入了感測器訊息檔案 Sensor_msg。

from sailboat_message.msg import Sensor_msg

隨後，程式在第三個函數中使用了感測器資料。

def sensorCallback(msg): #sailboat_message::Sensor_msg
    global sensor_submsg 
    sensor_submsg[0] = msg.Roll
    sensor_submsg[1] = msg.Yaw
    sensor_submsg[2] = msg.AWA

這個函數引數 msg 是從 ROS 訂閱得到的，我們可以在主函數中看到函數的呼叫：

rospy.Subscriber("sensor", Sensor_msg, sensorCallback)

回到第三個函數，感測器資料檔案位於

~/sailboat_ws/src/Sailboat-Ros/Message/sailboat_message/msg

開啟這個檔案，我們可以看到感測器資料有這些：

Header header
float64 ux
float64 vy
float64 wz
float64 gx
float64 gy
float64 gz
float64 Posx
float64 Posy
float64 PosZ
float64 Roll
float64 Pitch
float64 Yaw
float64 AWA
float64 AWS
float64 TWA
float64 TWS

在範例檔案 example.py 中，sensorCallback 函數只使用了其中的三個資料 (Roll, Yaw, AWA)，如果我們需要其它感測器資料，就將需要的值在 sensorCallback 函數中賦值給全域性變數陣列 sensor_submsg，然後修改程式開頭全域性變數陣列的長度。例如，我希望使用相對風速 AWS，應當這樣做：

sensor_submsg = [0,0,0,0]

def sensorCallback(msg): #sailboat_message::Sensor_msg
    global sensor_submsg 
    sensor_submsg[0] = msg.Roll
    sensor_submsg[1] = msg.Yaw
    sensor_submsg[2] = msg.AWA
    sensor_submsg[3] = msg.AWS

這樣，主函數需要使用 AWS 時，只需呼叫全域性變數 sensor_submsg[3] 即可。

2. 將資料輸出到 rqt

如果希望將一些自己定義的資料輸出到 rqt 以方便偵錯，那麼需要修改檔案 spare_function_out.msg，檔案目錄如下

/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg

開啟檔案後，可以看到目前裡面只有帆和舵的資料

float64 sail
float64 rudder

如果我們在主函數中自己定義了一個名稱為 is_tacking 的布林變數，來判斷帆船當前處於順風狀態還是逆風，需要將 spare_function_out.msg 檔案修改為

float64 sail
float64 rudder
bool is_tacking_publish

此外，還需要將自己寫的 python 程式，例如 example.py 的主函數改為

try:
        while not rospy.is_shutdown():
            mach_np = [ra, sa, 0]
            out_np = [ra, sa, is_tacking]
            #para_np = [0,0,0,0,0,0,0]
            # 後面的內容此處省略

然後將主函數外部的第二個函數修改為

def getOutput(msg): #spare_function::spare_function_out
    out_pub = spare_function_out()
    out_pub.rudder = msg[0]
    out_pub.sail = msg[1]
    out_pub.is_tacking_publish = msg[2]
    return out_pub

然後在 rqt 中就可以看到輸出了。上述範例中，可以將 is_tacking_publish 和 is_tacking 兩個變數設定為相同的名稱，也可以不同，這裡是為了方便看清變數之間的關係。

例如，為了顯示一下效果，這裡將 is_tacking 定為 true

try:
        while not rospy.is_shutdown():
        	ra = 0.5
        	sa = 0.5
        	is_tacking = True
            mach_np = [ra, sa, 0]
            out_np = [ra, sa, is_tacking]
            #para_np = [0,0,0,0,0,0,0]
            # 後面的內容此處省略

因為按照上述操作修改了 msg 檔案，所以需要將工作空間重新編譯。
提示：只修改 python 型別的檔案後不需要重新編譯，修改其它檔案則需要編譯。

cd ~/sailboat_ws
catkin clean
提示是否要清理，輸入 y ，回車確認
catkin build

重新編譯完成後，按照第一章的步驟啟動圖形化介面、執行 python 檔案。在 rqt 中勾選 spare_function_out 並展開，可以看到此時已經可以顯示自己定義的引數 is_tacking_publish 了。
rqt

3. 使用動態引數

動態引數就是在程式執行過程中可以實時修改的引數。程式將會使用最新修改的動態引數進行計算。假如我寫了一個讓帆船保持首向的程式，我希望在帆船執行一段時間後修改目標首向，但又不想把 python 程式開啟手動修改對應的變數。這時可以使用動態引數來簡化這一操作。此外，動態引數還可以在實船實驗時實時調整 PID 引數。
與動態引數相關的檔案及目錄如下：

檔案 spare_function_para.msg 目錄如下：
~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg
檔案 spare_function.cfg 目錄如下：
~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/cfg

如需增加或修改動態引數變數名，先在 spare_function_para.msg 中修改或新增，然後在 spare_function.cfg 中修改或新增。在 spare_function.cfg 中，若將目標首向 oyaw 的下限和上限修改為 -180 和 180，將預設值修改為 91，可以這樣：

檔案 spare_function.cfg

gen.add("PC_Ctrl",   bool_t,   0, "PC_Ctrl",  False)

gen.add("oyaw", double_t, 0, "oyaw",    91,  -180,   180)

gen.add("rudderP", double_t, 0, "rudderP",    0.5,  -10,   10)
gen.add("rudderI", double_t, 0, "rudderI",    0,    -10,   10)
gen.add("rudderD", double_t, 0, "rudderD",    0.2,  -10,   10)

gen.add("sailP", double_t, 0, "sailP",    -0.44,  -10,   10)
gen.add("sailI", double_t, 0, "sailI",    0,    -10,   10)
gen.add("sailD", double_t, 0, "sailD",    0,  -10,   10)

如果對動態引數的變數名進行了修改或增加，則還需要在 python 檔案中修改 getOutParaPut 函數和 getConfigCallback 函數，檔案開頭的全域性變數 para_cfg 也需要相應的修改。

使用動態引數時，只需要使用陣列 para_cfg 即可。

同樣的，如果對 msg 檔案或 cfg 檔案進行修改，則需要重新編譯。

例如，除了修改動態引數的預設值和範圍，我還希望在主程式中使用動態引數，並將目標航向輸出到 rqt 裡，則可以這樣做：

python檔案：新增輸出

try:
        while not rospy.is_shutdown():
            ra = 0.5
            sa = 0.5
            is_tacking = True
	    	oyaw = para_cfg[1]
            mach_np = [ra, sa, 0]
            out_np = [ra, sa, is_tacking, oyaw]
            #para_np = [0,0,0,0,0,0,0]

python檔案：新增輸出

def getOutput(msg): #spare_function::spare_function_out
    out_pub = spare_function_out()
    out_pub.rudder = msg[0]
    out_pub.sail = msg[1]
    out_pub.is_tacking_publish = msg[2]
    out_pub.oyaw = msg[3]
    return out_pub

spare_function_out.msg

float64 sail
float64 rudder
bool is_tacking_publish
float64 oyaw

然後重新編譯工作空間，啟動圖形化介面，執行 python 檔案。
rqt 介面中，在上方的 Dynamic Reconfigure 模組中點選 refresh 重新整理一下動態參數列。然後點選 example_ 後面一串數位的那個選項，可以開啟剛剛設定好的動態引數。點選 wind_simulation 則可以調整模擬風速和風向的動態引數。
rqt-Refresh 在這裡插入圖片描述在 Topic Monitor 中勾選 spare_function_out 和 spare_function_para，檢視相應的資料。然後拖動動態引數的遊標，可以看到下邊的資料也會跟著改變。

實船實驗

注意：如果要在實船實驗中調整動態引數，則需要在實驗前將自己電腦的的名字寫入船上主機板的某組態檔的開頭，這樣才可以獲取修改動態引數的許可權。詳見疑問彙總「多機通訊.md」。如果沒有這個檔案，請和助教聯絡獲取。
實驗前，需要將所有修改過的檔案複製到船上主機板中：請注意區分本機埠和遠端登入的船上主機板埠

登入 sjtu151 埠刪除檔案 spare_function.cfg
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/cfg/
rm -f spare_function.cfg
在本機埠複製檔案給sjtu151
scp ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/cfg/spare_function.cfg sjtu151@192.168.1.51:/home/sjtu151/sailboat-ws/src/Sailboat-Ros/Class/spare_function/cfg/

登入sjtu151埠刪除檔案 spare_function_para.msg
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/
rm -f spare_function_para.msg
在本機埠複製檔案給sjtu151
scp ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg/spare_function_para.msg
sjtu151@192.168.1.51:/home/sjtu151/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/

登入sjtu151埠刪除檔案 spare_function_out.msg
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/
rm -f spare_function_out.msg
在本機埠複製檔案給sjtu151
scp ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg/spare_function_out.msg sjtu151@192.168.1.51:/home/sjtu151/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/

登入sjtu151埠刪除 python 檔案 
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/scripts
rm -f python檔名
在本機埠複製檔案給sjtu151
scp ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/scripts/example.py sjtu151@192.168.1.51:/home/sjtu151/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts/

然後登入sjtu151埠重新編譯
cd ~/sailboat_ws
catkin clean
catkin build

然後依次啟動船上程式和岸上程式，然後記錄資料即可。啟動岸上程式之後，你的電腦將會跳出 rqt 和 rviz 的圖形化介面，之後所有的操作與模擬時一致。

無人帆船模擬及實船實驗步驟

目錄