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