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口，机器人否是远程模式。
