模擬實驗

雙帆船的帆機通過繩索與兩個帆連接，帆的實際角度會受到風的影響，在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 中點擊相應的選項可以顯示帆船的路徑，

在 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 了，

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 則可以調整模擬風速和風向的動態引數，
在 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 的圖形化界面，之后所有的操作與模擬時一致，