python實現CS機器人30001端口數據解析
1.簡介
30001 端口可用于獲取機器人的各種狀態及數據，CS機器人會以 10HZ 的頻率向 30001 端口 發送機器人的狀態數據，本文解析”機器人是否上電“和“機器人當前實際關節角度”為例說明如何解析30001端口返回的數據。
2.操作流程
為了更容易理解，以下先通過網絡調試助手，手動解析說明后再通過python實現自動解析，詳細報文請參考”機器人狀態報文表“(“機器人狀態報文表“聯系艾利特技術人員獲取)
2.1 通過調試軟件手動解析
1.通過網絡調試助手連接CS機器人30001端口后，機器人會以 10HZ 的頻率向 30001 端口 發送數據，如圖2-1-1所示：為機器人FB1網口IP，以16進制格式接收。
圖2-1-1
2.1.1 整個機器人狀態報文截取
1.通過”機器人狀態報文表”得知機器人狀態數據總共有1157位字節(5+53+347+101+445+88+10+37+43+28=1157)，其中報文頭：前面4位字節為報文長度，第5位字節為報文類型=16。
圖2-1-2
2.首先我們需要找到報文頭，因為調試助手是以十六進制接收機器人返回的數據，”機器人狀態報文表”中的報文的長度和報文類型(16)數據為十進制，所以首先把整個報文長度(1157)和報文類型=(16)轉換為16進制，方便尋找報文頭，十進制1157轉換成十六進制為 ”485“ 又因為報文頭在數據中占四個字節所以整個報文頭長度為”00 00 04 85“，十進制16轉換成十六進制為“10”，如圖2-1-3：
圖2-1-3
3.通過上面的報文頭數據轉換后我們在機器人返回的數據中找到整個報文頭長度為”00 00 04 85“和報文類型”10“，從報文頭”00 00 04 85 10“的第1位往后截取到1157位就是整個機器人狀態的報文，如圖2-1-4所示：
機器人返回的數據：# RECV HEX
00 00 00 0B C8 00 00 00 00 00 00 00 00 00 04 85 10 00 00 00 35 00 00 00 00 00 0B 76 45 E1 01 01 00 00 00 00 00 03 00 3F F0 00 00 00 00 00 00 3F F0 00 00 00 00 00 00 3F F0 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 5B 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 3F F9 21 FB 54 52 45 50 3F F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 ......到1157位字節為整個機器人狀態的報文
圖2-1-4
2.1.2 機器人模式解析
1.當我們需要解析機器人的某一種狀態時,首先需要找到這個狀態的子報文頭即可，例如通過”機器人狀態報文表”得知”機器人模式”子報文長度為53且占4個字節如圖2-1-5所示，所以轉換成十六進制為“00 00 00 35”。
圖2-1-5
2. 首先需要找到機器人模式子報文長度“00 00 00 35”和子報文類型”00“，然后從報文頭“00 00 00 35 00“第1位字節截取到53位字節為機器人模式數據的報文如圖2-1-6所示(紅到紫)：
機器人返回的數據：# RECV HEX
00 00 00 0B C8 00 00 00 00 00 00 00 00 00 04 85 10 00 00 00 35 00 00 00 00 00 0B 76 45 E1 01 01 01 00 00 00 00 03 00 3F F0 00 00 00 00 00 00 3F F0 00 00 00 00 00 00 3F F0 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 5B 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 3F F9 21 FB 54 52 45 50 3F F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
圖2-1-6
3. 比如想知道機器人是否上電，通過”機器人狀態報文表”得知”機器人是否上電”在機器人模式數據報文中的第16位字節占一個字節如圖2-1-7所示，所以“機器人模式報文”中的第16位為1時機器人上電，為0時機器人下電，如圖2-1-8所示“機器人模式報文”中的第16位為01機器人已經上電。
圖2-1-7
機器人返回的數據：# RECV HEX
00 00 00 0B C8 00 00 00 00 00 00 00 00 00 04 85 10 00 00 00 35 00 00 00 00 00 0B 76 45 E1 01 01 01 00 00 00 00 03 00 3F F0 00 00 00 00 00 00 3F F0 00 00 00 00 00 00 3F F0 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 5B 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 3F F9 21 FB 54 52 45 50 3F F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 EF 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
圖2-1-8
2.1.3 關節數據解析
1.當需要解析機器人實際關節角度時，一樣的方法首先需要找到關節數據的子報文頭，通過”機器人狀態報文表”得知”關節數據”子報文長度為347字節且占前面4個字節,子報文類型為1占第4個字節，如圖2-1-9所示，所以轉換成十六進制分別為“00 00 01 5B”,”01“。
圖2-1-9
2.首先需要找到關節數據子報文長度“00 00 01 5B”和子報文類型”01“，然后從報文頭“00 00 01 5B 01“第1位字節截取到347位字節為機器人模式數據的報文如圖2-1-10所示(紅到紫)
機器人返回的數據：# RECV HEX
00 3F F0 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 5B 01 BF B7 91 B4 46 08 38 A1 BF B7 91 B4 46 08 38 A1 BE B9 A1 C1 77 44 F2 AF FF FF B0 37 FF FF B0 37 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00BF F9 0E 79 07 CC 04 57 BF F9 0E 79 07 CC 04 57 00 00 00 00 00 00 00 00 00 04 6C CE 00 04 6C CE 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 80 00 00 00 FD 00 00 00 00 BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 3F F9 21 FB 54 52 45 50 3F F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 00 00 00 65 04 3F DE 32 18 79 45 90 86 BF C8 89 3E F4 BA 52 96 3F DF F
圖2-1-10
3.由圖2-1-9可知關節數據中的347字節中前面五個字節為子報文頭,報文頭后面分別為六個關節的數據每個關節數據有57個字節：子報文頭(5)+關節1數據(57)+關節2數據(57)+關節3數據(57)+關節4數據(57)+關節5數據(57)+關節6數據=關節數據(347)，為了便于解析每個關節的數據以顏色區分如圖2-1-11所示：
機器人返回的數據：# RECV HEX
00 3F F0 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 5B 01 關節1數據: BF B7 91 B4 46 08 38 A1 BF B7 91 B4 46 08 38 A1 BE B9 A1 C1 77 44 F2 AF FF FF B0 37 FF FF B0 37 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 關節2數據: BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 關節3數據: BF F9 0E 79 07 CC 04 57 BF F9 0E 79 07 CC 04 57 00 00 00 00 00 00 00 00 00 04 6C CE 00 04 6C CE 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 80 00 00 00 FD 00 00 00 00 關節4數據: BF F9 21 FB 54 52 45 50 BF F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 關節5數據: 3F F9 21 FB 54 52 45 50 3F F9 21 FB 54 52 45 50 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 關節6數據: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 41 F0 00 00 00 00 00 00 FD 00 00 00 00 00 00 00 65 04 3F DE 32 18 79 45 90 86 BF
圖2-1-11
4.通過”機器人狀態報文表”得知，如圖2-1-12得知每個關節的實際關節角度數據在每個關節數據的前8個字節，所以：關節1的實際關節角度為:”BF B7 91 B4 46 08 38 A1“，關節2的實際關節角度為:”BF F9 21 FB 54 52 45 50“，關節3的實際關節角度為:”BF F9 0E 79 07 CC 04 57“，關節4的實際關節角度為:"BF F9 21 FB 54 52 45 50",關節5的實際關節角度為:”3F F9 21 FB 54 52 45 50“，關節6的實際關節角度為:”00 00 00 00 00 00 00 00“，
圖2-1-12
5.由于網絡調試助手是以16進制接收機器人返回的數據，所以需要把16進制轉換成浮點數(8個字節為雙精度浮點數)，可以在網上下載”浮點數十六進制轉換器“對數據進行轉換，下面以關節1的實際關節角度:”BF B7 91 B4 46 08 38 A1“為例對數據進行轉換，得到的數據正確，說明解析關節1實際關節角度正確，如圖2-1-13所示：
圖2-1-13
6. 通過”浮點數十六進制轉換器“對每個關節實際角度數據依次進行轉換得：
關節1實際關節角度：BF B7 91 B4 46 08 38 A1 = -0.0920670195557
關節2實際關節角度：BF F9 21 FB 54 52 45 50 = -1.5707963270000
關節3實際關節角度：BF F9 0E 79 07 CC 04 57 = -1.5660333923882
關節4實際關節角度：BF F9 21 FB 54 52 45 50 = -1.5707963270000
關節5實際關節角度：3F F9 21 FB 54 52 45 50 = 1.5707963270146
關節6實際關節角度：00 00 00 00 00 00 00 00 = 0.00
四舍五入保留兩位小數后與機器人示教器上顯示的數據一樣說明解析正確，如圖2-1-14，單位(弧度)。
圖2-1-14
2.2 通過python自動解析
通過以上手動解析的方法對機器人狀態報文以及數據格式基本了解后下面通過python對30001端口數據進行解析，還是以”機器人是否上電“和“機器人當前實際關節角度”為例說明。
2.2.1 程序思路
1.把機器人模式子報文長度”53“和關節數據子報文長度”347“按照大端字節序打包成4個字節的形制存放相應的變量中(CS機器人數據協議采用大端模式，子報文長度占4個字節，機器人以字節流的格式返回，把子報文長度轉換成字節流的格式方便程序中使用）；
2.連接機器人30001端口后循環接收機器人返回的數據，判斷接收的數據中是否有機器人模式子報文頭和關節數據子報文頭；
3.解析”機器人是c)“后再次去掉”子報文類型(1個字節)“還剩48個字節=數據C,通過”機器人狀態報文表”得知”機器人是否上電“位于”機器人模式”報文中的第16位，由于數據C中已經去掉子報文頭(“子報文長度4個字節”和子報文類型1個字節)，因此機器人是否上電“位于數據C中的第11位，所以需要把C[10:11]換成成整數后判斷是否為1，為1時機器人已上電，為0時機器人已下電；
4.解析“關節實際角度”時：一樣的方法以關節數據報文頭分割，數據分割后得到數據為[A，B],先判斷數據B的長度是否大于343(判斷關節數據是否完整)，因為關節數據子報文頭占4個字節，把關節子報文頭分割還剩343個字節，如果數據B的長度大343時，那么可以截取數據B的前343位字節做為關節數據報文C數據，如:C=B[1:343](再次去掉前面一個字節：子報文類型)，因為分割掉子“報文長度(4個字節)“后再次去掉”子報文類型(1個字節)“還剩342個字節=數據C,通過”機器人狀態報文表”得知數據C前57位是關節1的數據，關節1數據后57位是關節2數據，關節2數c據后57位是關節3數據，依此類推到關節6數據，每個關節的前8個字節是相應的實際關節角度，因此解析數據C的前8位字節為關節1的實際關節角度后去掉關節1的所有數據后得數據D，解析數據D的前8位字節為關節2的實際關節角度后去掉關節2的所有數據后得數據F，依此類推到關節6,如：關節1=C[0:8],D=C[57:],關2=D[0:8],F=[57:]，依此類推得到六個關節的數據，最后將截取的數據轉換成雙精度浮點數；
2.2.2 程序代碼
python解析30001端口機器人模式及關節位置案例.py
3.常見問題解答
1.需要連接CS機器人FB1網口，機器人切換遠程模式；
2.機器人數據協議采用大端模。(例如: 00 00 08，小端模式:08 00 00 ,大端模式:00 00 08)；
3.機器人返回的位置數據以米和弧度為單位；
4.端口連接不上時，檢查一下網絡是不是沒有連接上，可以在示教器網絡設置頁面查看，或者使用電腦ping一下看看能否ping通，網口是否正常連接的FB1口，機器人否是遠程模式。
