xbot_checker3.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # @Author: roc
4 # @Date: 2019-01-17 11:59:04
5 # @Last Modified by: wangpeng@droid.ac.cn
6 # @Last Modified time: 2019-01-19 22:16:10
7 ###########################################################
8 #该软件适用版本为2.2.0和3.0.0
9 #该软件实现了以下功能：
10 #１、判断测试机是否连接了串口设备，并输出串口设备清单；
11 #２、如果连接了串口设备，确定串口设备与PCB1(PCB1),PCB2(PCB2),rplidar的对应关系
12 #３、输出从串口设备读取的一段完整的串口数据帧
13 #４、按照串口协议解析出数据帧中对应码段的真实数据
14 #５、将串口映射表文件写入系统的/etc/udev/rules.d/目录下
15 #
16 ###########################################################
17 
18 
19 import serial.tools.list_ports
20 import string,os,commands,struct
21 import serial
22 import string
23 import binascii
24 import time
25 from time import sleep
26 
27 
28 debug = False
29 
30 def getCameraId():
31 
32  cmd = "ls /dev/video* > vs.out"
33  os.system(cmd)
34  vf = open('vs.out','r')
35  vs = vf.readlines()
36  vf.close()
37  vn = len(vs)
38  # print vs
39  camera = False
40  for dev in vs:
41  vd = dev[:-1]
42  cmd = "udevadm info "+vd+" |grep 'ID_VENDOR_ID=0bda' > vs.info"
43  os.system(cmd)
44  infof = open("vs.info",'r')
45  info = infof.readlines()
46  infof.close()
47  # print info
48  if len(info)!=0:
49  camera = True
50  if debug:
51  print 'camera video id: '+vs[i][-2]
52  break
53 
54  rsense = False
55  for dev in vs:
56  vd = dev[:-1]
57  cmd = "udevadm info "+vd+" |grep RealSense > vs.info"
58  os.system(cmd)
59  infof = open("vs.info",'r')
60  info = infof.readlines()
61  infof.close()
62  # print info
63  if len(info)!=0:
64  rsense = True
65  if debug:
66  print 'RealSense video id: '+vs[i][-2]
67  break
68 
69  return camera,rsense
70 
71 
72 cam,rs = getCameraId()
73 if cam:
74  print '\033[1;32m[√] 人脸识别摄像头 \033[0m'
75 else:
76  print '\033[1;31m[×] 人脸识别摄像头 \033[0m'
77 
78 if rs:
79  print '\033[1;32m[√] RealSense摄像头 \033[0m'
80 else:
81  print '\033[1;31m[×] RealSense摄像头 \033[0m'
82 
83 
84 rplidar_pluged = False
85 arm_pluged = False
86 
87 PCB1_pluged = False
88 label_PCB1 = 'aa5530'
89 ss_PCB1 = ''
90 
91 PCB2_pluged=False
92 label_PCB2 = 'aa5538'
93 ss_PCB2 = ''
94 
95 # print '\033[1;34m 2、输出已接入串口设备清单... \033[0m'
96 
97 port_list = list(serial.tools.list_ports.comports())
98 port_num = len(port_list)
99 if port_num == 0:
100  print '\033[1;31m[×] 激光雷达\033[0m'
101  print '\033[1;31m[×] 机械臂 \033[0m'
102  print '\033[1;31m[×] PCB1 \033[0m'
103  print '\033[1;31m[×] PCB2 \033[0m'
104  exit()
105 else:
106  if debug:
107  print '当前插入了'+str(port_num)+'个串口。'
108  print '串口信息清单如下：'
109  print '----------------------------------------------------------------'
110  for i in range(0,port_num):
111  print port_list[i]
112  print '----------------------------------------------------------------'
113 
114 
115 label_rplidar = 'CP2102'
116 for i in range(0, port_num):
117  if( label_rplidar in str(port_list[i])):
118  rplidar_pluged = True
119  port_list.remove(port_list[i])
120  port_num = port_num - 1
121  break
122 if not rplidar_pluged:
123  print '\033[1;31m[×] 激光雷达 \033[0m'
124 else:
125  print '\033[1;32m[√] 激光雷达 \033[0m'
126  os.system("cp udev/71-rplidar.rules /etc/udev/rules.d/")
127  print '\033[1;32m |----[√] 激光雷达端口映射文件写入系统 \033[0m'
128 
129 
130 label_arm = 'USB2.0-Serial'
131 for i in range(0, port_num):
132  if( label_arm in str(port_list[i])):
133  arm_pluged = True
134  port_list.remove(port_list[i])
135  port_num = port_num - 1
136  break
137 if not arm_pluged:
138  print '\033[1;31m[×] 机械臂 \033[0m'
139 else:
140  print '\033[1;32m[√] 机械臂 \033[0m'
141  os.system("cp udev/72-arm.rules /etc/udev/rules.d/")
142  print '\033[1;32m |----[√] 机械臂端口映射文件写入系统 \033[0m'
143 
144 
145 
146 def recv(serial):
147  start = time.time()
148  while True:
149  data = serial.read_all()
150  if data == '':
151  if time.time()-start >10:
152 
153  break
154 
155  else:
156  break
157  sleep(0.02)
158  return data
159 
160 port_num = len(port_list)
161 if port_num == 0:
162  exit()
163 
164 for i in range(0, port_num):
165  ss = ''
166  dev_name = str(port_list[i]).split(' ')[0]
167  try:
168  serial1 = serial.Serial(dev_name, 115200, timeout=0.5)
169 
170  except Exception as e:
171  raise e
172  # print e
173  finally:
174  pass
175 
176 
177  while len(ss)/2<151 :
178  s =recv(serial1)
179 
180  if s != b'' :
181  data= str(binascii.b2a_hex(s))
182 
183  ss = ss+data
184  else:
185  print '\033[1;31m 已连接串口超过10秒未接收到数据，请确认是否未供电。 \033[0m'
186  break
187 
188 
189 
190  if(label_PCB2 in ss ):
191  PCB2_dev_name = dev_name
192  ss_PCB2 = 'aa55'+ss.split('aa55')[1]
193  PCB2_pluged = True
194 
195  elif(label_PCB1 in ss):
196 
197  PCB1_dev_name = dev_name
198  ss_PCB1 = 'aa55'+ss.split('aa55')[1]
199  PCB1_pluged = True
200 
201 
202 
203 def check(s):
204  l=len(s)
205  i=1
206  x=int(s[0:2],16)
207  while i<l/2-1:
208  x=x^int(s[i*2:i*2+2],16)
209  i=i+1
210  if x==int(s[-2:],16):
211  return True
212  else:
213  return False
214 
215 
216 print '\033[1;32m[ ] PCB1 \033[0m'
217 if(PCB1_pluged):
218  print '\033[1;32m |----[√] PCB1接入，端口名： \033[0m'+PCB1_dev_name
219  print '\033[1;32m |----[√] 接收PCB1数据帧： \033[0m' +ss_PCB1
220  if(check(ss_PCB1)):
221  print '\033[1;32m |----[√] PCB1数据校验 \033[0m'
222  payload = ss_PCB1[6:-2]
223  label = payload[0:2]
224  if(label != '10'):
225  print '\033[1;31m |----[×] PCB1标志位 \033[0m'
226  else:
227  left_encoder = int(payload[2:6],16)
228 
229  else:
230  print '\033[1;32m |----[×] PCB1数据校验 \033[0m'
231 
232  cmd = 'udevadm info -a -n '+PCB1_dev_name+ '|grep ATTRS{serial}'
233  (status, output) = commands.getstatusoutput(cmd)
234  PCB1_serial = output.split('"')[1]
235  print '\033[1;32m |----[√] PCB1芯片序列号： \033[0m'+PCB1_serial
236  f=open("udev/73-xbot.rules",'r') #your path!
237  s= f.read()
238  f.close()
239  s= s+',ATTRS{serial}==\"'+PCB1_serial+'\"'
240  f=open("/etc/udev/rules.d/73-xbot.rules",'w')
241  f.write(s)
242  f.close()
243  print '\033[1;32m |----[√] PCB1端口映射文件写入系统 \033[0m'
244 else:
245  print '\033[1;31m |----[×] PCB1接入 \033[0m'
246 
247 
248 
249 
250 print '\033[1;32m[ ] PCB2 \033[0m'
251 if(PCB2_pluged):
252  print '\033[1;32m |----[√] PCB2接入，端口名： \033[0m'+PCB2_dev_name
253  print '\033[1;32m |----[√] 接收PCB2数据帧： \033[0m' +ss_PCB2
254  if(check(ss_PCB2)):
255  print '\033[1;32m |----[√] PCB2数据校验 \033[0m'
256  payload = ss_PCB2[6:-2]
257  label = payload[0:2]
258  if(label != '11'):
259  print '\033[1;31m |----[×] PCB2标志位 \033[0m'
260  else:
261  # print len(payload)
262  yaw_platform = struct.unpack('B', payload[2:4].decode('hex'))[0]
263  pitch_platform = struct.unpack('B', payload[4:6].decode('hex'))[0]
264  volume = struct.unpack('B', payload[6:8].decode('hex'))[0]
265  acc_x = struct.unpack('<h', payload[8:12].decode('hex'))[0]
266  acc_y = struct.unpack('<h', payload[12:16].decode('hex'))[0]
267  acc_z = struct.unpack('<h', payload[16:20].decode('hex'))[0]
268  gyro_x = struct.unpack('<h', payload[20:24].decode('hex'))[0]
269  gyro_y = struct.unpack('<h', payload[24:28].decode('hex'))[0]
270  gyro_z = struct.unpack('<h', payload[28:32].decode('hex'))[0]
271  mag_x = struct.unpack('<h', payload[32:36].decode('hex'))[0]
272  mag_y = struct.unpack('<h', payload[36:40].decode('hex'))[0]
273  mag_z = struct.unpack('<h', payload[40:44].decode('hex'))[0]
274  yaw = -struct.unpack('<f', payload[44:52].decode('hex'))[0]
275  pitch = struct.unpack('<f', payload[52:60].decode('hex'))[0]
276  roll = struct.unpack('<f', payload[60:68].decode('hex'))[0]
277  q1 = struct.unpack('<f', payload[68:76].decode('hex'))[0]
278  q2 = struct.unpack('<f', payload[76:84].decode('hex'))[0]
279  q3 = struct.unpack('<f', payload[84:92].decode('hex'))[0]
280  q4 = struct.unpack('<f', payload[92:100].decode('hex'))[0]
281  error_status = struct.unpack('B', payload[100:102].decode('hex'))[0]
282  time_stamp = struct.unpack('<H', payload[102:106].decode('hex'))[0]
283  print '\033[1;32m |----[√] PCB2数据解析 \033[0m'
284  else:
285  print '\033[1;31m |----[×] PCB2数据校验 \033[0m'
286 
287  cmd = 'udevadm info -a -n '+PCB2_dev_name+ '|grep ATTRS{serial}'
288  (status, output) = commands.getstatusoutput(cmd)
289  PCB2_serial = output.split('"')[1]
290  print '\033[1;32m |----[√] PCB2芯片序列号： \033[0m'+PCB2_serial
291  f=open("udev/74-sensor.rules",'r') #your path!
292  s= f.read()
293  f.close()
294  s= s+',ATTRS{serial}==\"'+PCB2_serial+'\"'
295  f=open("/etc/udev/rules.d/74-sensor.rules",'w')
296  f.write(s)
297  f.close()
298  print '\033[1;32m |----[√] PCB2端口映射文件写入系统 \033[0m'
299 
300 
301 else:
302  print '\033[1;31m |----[×] PCB2接入 \033[0m'
303 
xbot_checker3.check
def check(s)
Definition: xbot_checker3.py:203
xbot_checker3.recv
def recv(serial)
Definition: xbot_checker3.py:146
xbot_checker3.getCameraId
def getCameraId()
Definition: xbot_checker3.py:30

xbot_tools
Author(s):
autogenerated on Sat Oct 10 2020 03:28:22