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