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