lidarbroadcast.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy, tf
4 import serial, math
5 import usbdiscover
6 from sensor_msgs.msg import LaserScan
7 from nav_msgs.msg import Odometry
8 from dynamic_reconfigure.server import Server
9 import dynamic_reconfigure.client
10 from xaxxon_openlidar.cfg import XaxxonOpenLidarConfig
11 
12 headeroffset = None # set by cfg
13 masks = [] # set by cfg
14 minimum_range = 0.0 # set by cfg
15 maximum_range = 40.0 # set by cfg
16 readInterval = 0.0014 # must match firmware TODO: read from device on init
17 rpm = 180 # rev speed, set by cfg, 250 max
18 nominalcycle = 1/(rpm/60.0)
19 dropscan_degpersec = 0
20 parkoffset = 0 # used only by device firmware, set by cfg
21 rotateforwardoffset = 0
22 enable = True
23 ser = None
24 lidarrunning = False
25 
26 dropScanTurnRateThreshold = 0 # 0 = disabled
27 lastodomth = None
28 lastodomtime = 0
29 turning = False
30 turnrate = 0 # radians per second
31 
32 forward_offset = headeroffset
33 park_offset = parkoffset
34 read_frequency = 0
35 rotate_forward_offset = rotateforwardoffset
36 rpm_ = rpm
37 
38 scan = None
39 rebroadcastscan = None
40 lastscantime = 0
41 BROADCASTLAST = True # continue to broadcast last scan while lidar disabled
42 DIR = 1 # motor direction (1=CW/RHR+ with motor @ bottom, 0=CCW, ROS default)
43 DEBUGOUTPUT = True
44 
45 def cleanup():
46  global ser, lidarrunning
47 
48  ser.write("f\n") # stop lidar
49  lidarrunning = False
50 
51  if DEBUGOUTPUT:
52  rospy.sleep(0.5)
53  ser.reset_input_buffer()
54  ser.write("z\n") # get i2c_error count
55  rospy.sleep(0.1)
56  while ser.inWaiting() > 0:
57  line = "SCAN: "+ser.readline().strip()
58  rospy.loginfo(line)
59 
60  ser.close()
61  ser = None
62  rospy.loginfo("SCAN: lidar disabled, shutdown");
63 
64 
65 def odomCallback(data):
66  global turning, turnrate, lastodomth, lastodomtime
67 
68  if dropScanTurnRateThreshold == 0:
69  return
70 
71  quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
72  data.pose.pose.orientation.z, data.pose.pose.orientation.w )
73  odomth = tf.transformations.euler_from_quaternion(quaternion)[2] # Z rotation
74 
75  now = rospy.get_time()
76 
77  if lastodomtime == 0:
78  lastodomth = odomth
79  lastodomtime = now
80  return
81 
82  turnrate = (odomth-lastodomth)/(now-lastodomtime) # radians per second
83 
84  if abs(turnrate) > dropScanTurnRateThreshold:
85  turning = True
86  else:
87  turning = False
88 
89  lastodomth = odomth
90  lastodomtime = now
91 
92 
93 def dynamicConfigCallback(config, level):
94  global newheaderoffset, minimum_range, maximum_range
95  global dropScanTurnRateThreshold
96  global forward_offset, rpm_, park_offset, read_frequency, rotate_forward_offset
97  global enable
98 
99  # if DEBUGOUTPUT:
100  # print(config)
101 
102  updateMasks(config.masks)
103 
104  minimum_range = config.minimum_range
105  maximum_range = config.maximum_range
106 
107  dropScanTurnRateThreshold = math.radians(config.dropscan_turnrate)
108 
109  forward_offset = config.forward_offset
110  rpm_ = config.rpm
111  park_offset = config.park_offset
112  read_frequency = config.read_frequency
113  rotate_forward_offset = config.rotate_forward_offset
114  enable = config.lidar_enable
115 
116  if not ser == None:
117  updateRotateForwardOffset() # needs to be before updateHeaderOffset
118  updateHeaderOffset()
119  updateRPM()
120  updateParkOffset()
121  updateReadInterval()
122 
123  return config
124 
125 
126 def updateHeaderOffset():
127  global headeroffset
128 
129  if headeroffset == forward_offset:
130  return
131 
132  headeroffset = forward_offset
133 
134  n = int(headeroffset*10)
135 
136  if DEBUGOUTPUT:
137  print("sending new headeroffset to device: "+str(n))
138  val1 = n & 0xFF
139  val2 = (n >>8) & 0xFF
140  ser.write("k")
141  ser.write(chr(val1))
142  ser.write(chr(val2))
143  ser.write("\n")
144 
145 # def getHeaderOffset():
146  # global headeroffset
147 
148  # ser.write("i\n") # get header offset
149  # rospy.sleep(0.1)
150  # while ser.inWaiting() > 0:
151  # line = ser.readline().strip()
152  # print(line)
153  # headeroffset = float(line.replace("<","").replace(">",""))*360.0
154  # print("headeroffset received from device: "+str(headeroffset))
155 
156  # client.update_configuration({"header_offset":headeroffset})
157 
158 
159 def updateMasks(string):
160  global masks
161 
162  # if DEBUGOUTPUT:
163  # print("updating masks: "+string)
164 
165  masks = []
166 
167  try:
168  strmasks = string.split()
169  for s in strmasks:
170  masks.append(float(s))
171  except:
172  rospy.logerr("error parsing masks")
173 
174  if not (len(masks) % 2) == 0:
175  rospy.logerr("uneven number of masks")
176  masks=[]
177 
178  updateRotateForwardOffset()
179 
180 
181 def updateRPM():
182  global rpm, nominalcycle
183 
184  if rpm == rpm_:
185  return
186 
187  rpm = rpm_
188  nominalcycle = 1/(rpm/60.0)
189 
190  if DEBUGOUTPUT:
191  print("sending rpm to device: "+str(rpm))
192 
193  ser.write("r"+chr(rpm)+"\n")
194 
195 
196 def updateParkOffset():
197  global parkoffset
198 
199  if parkoffset == park_offset:
200  return
201 
202  parkoffset = park_offset
203  n = int(parkoffset*10)
204 
205  if DEBUGOUTPUT:
206  print("sending parkoffset to device: "+str(parkoffset))
207 
208  val1 = n & 0xFF
209  val2 = (n >>8) & 0xFF
210  ser.write("q")
211  ser.write(chr(val1))
212  ser.write(chr(val2))
213  ser.write("\n")
214 
215 
216 def updateReadInterval():
217  global readInterval
218 
219  interval = int(1.0/read_frequency*1000000)
220  if interval/1000000.0 == readInterval:
221  return
222 
223  readInterval = interval/1000000.0
224 
225  if DEBUGOUTPUT:
226  print("sending read interval to device: "+str(interval))
227 
228  val1 = interval & 0xFF
229  val2 = (interval >>8) & 0xFF
230  ser.write("t")
231  ser.write(chr(val1))
232  ser.write(chr(val2))
233  ser.write("\n")
234 
235 
236 def updateRotateForwardOffset():
237  global rotateforwardoffset, masks, headeroffset, forward_offset
238 
239  if rotateforwardoffset == rotate_forward_offset:
240  return
241 
242  if DEBUGOUTPUT:
243  print("setting rotate_forward_offset: "+str(rotate_forward_offset))
244 
245  i = 0
246  while i < len(masks):
247  # un-apply old rotate_forward_offset
248  masks[i] += rotateforwardoffset
249  masks[i+1] += rotateforwardoffset
250  if masks[i] >= 360 or masks[i+1] >=360:
251  masks[i] -=360
252  masks[i+1] -= 360
253 
254  # apply new rotate_forward_offset
255  masks[i] -= rotate_forward_offset
256  masks[i+1] -= rotate_forward_offset
257  if masks[i] < 0 or masks[i+1] < 0:
258  masks[i] += 360
259  masks[i+1] += 360
260 
261  i += 2
262 
263  # un-apply old rotate_forward_offset
264  forward_offset -= rotateforwardoffset
265  if forward_offset < 0:
266  forward_offset += 360
267 
268  # apply new rotate_forward_offset
269  forward_offset += rotate_forward_offset
270  if forward_offset >= 360:
271  forward_offset -= 360
272 
273  rotateforwardoffset = rotate_forward_offset
274 
275 
276 def readlidar(ser):
277 
278  global lidarrunning, scan, lastscantime, rebroadcastscan
279 
280  lidarrunning = True
281  # ser.write("r"+chr(rpm)+"\n")
282  ser.write("a\n") # start lidar
283  rospy.loginfo("SCAN: lidar enabled");
284 
285  # clear buffer
286  ser.reset_input_buffer()
287 
288  raw_data = []
289  lastscan = rospy.Time.now()
290  headercodesize = 4
291  current_time = 0
292  dropscan = False
293  scannum = 0
294  lastcount = 0
295  consecutivescandropped = 0
296 
297  while not rospy.is_shutdown() and ser.is_open and enable:
298 
299  # read data and dump into array, checking for header code 0xFF,0xFF,0xFF,0xFF
300  try:
301  ch = ser.read(1)
302  except:
303  pass
304 
305  # if timed out after TIMEOUT seconds, defined in usbdiscover.py)
306  if len(ch) == 0:
307  if scannum > 1:
308  rospy.logerr("no response from xaxxonlidar device")
309  scannum += 1
310  break
311  continue
312 
313  raw_data.append(ch)
314 
315  if turning:
316  dropscan = True
317 
318  if rebroadcastscan and BROADCASTLAST:
319  rebroadcast()
320 
321  if not ord(ch) == 0xFF:
322  continue
323  else:
324  ch = ser.read(1)
325  raw_data.append(ch)
326  if not ord(ch) == 0xFF:
327  continue
328  else:
329  ch = ser.read(1)
330  raw_data.append(ch)
331  if not ord(ch) == 0xFF:
332  continue
333  else:
334  ch = ser.read(1)
335  raw_data.append(ch)
336  if not ord(ch) == 0xFF:
337  continue
338 
339  ser.write("h\n") # send host hearbeat (every <10 sec minimum)
340 
341  # read count
342  low = ord(ser.read(1))
343  high = ord(ser.read(1))
344  count = (high<<8)|low
345 
346  """ read cycle """
347  c1 = ord(ser.read(1))
348  c2 = ord(ser.read(1))
349  c3 = ord(ser.read(1))
350  c4 = ord(ser.read(1))
351  cycle = ((c4<<24)|(c3<<16)|(c2<<8)|c1)/1000000.0
352 
353  """ host time """
354  current_time = rospy.Time.now() # - rospy.Duration(0.0) #0.015
355  rospycycle = current_time - lastscan
356  # cycle = rospycycle.to_sec()
357  lastscan = current_time
358 
359  rospycount = (len(raw_data)-headercodesize)/2
360 
361  scannum += 1
362 
363 
364  """ error monitoring, debug info """
365 
366  # output info every 10 scans
367  if DEBUGOUTPUT and scannum % 10 == 0:
368  msg = "SCAN "+str(scannum)+": cycle: "+str(cycle)+", count: "+str(count)
369  rospy.loginfo(msg)
370 
371  # warn if firmware sample count doesn't match
372  if not rospycount == count and scannum > 1 and DEBUGOUTPUT:
373  msg = "SCAN "+str(scannum)+": count/data mismatch: "+ str( rospycount-count )
374  rospy.logwarn(msg)
375 
376  # warn if sample count change
377  if abs(count - lastcount) > 2 and scannum > 4 and DEBUGOUTPUT:
378  msg = "SCAN "+str(scannum)+": count change from: "+ str(lastcount)+", to: "+str(count)
379  rospy.logwarn(msg)
380  lastcount = count
381 
382  # reset device if low sample count
383  if count<(nominalcycle/readInterval/2) and scannum > 4:
384  if DEBUGOUTPUT:
385  msg = "SCAN "+str(scannum)+": low data count: "+str(count)+", cycle: "+str(cycle)
386  msg += ", scan dropped"
387  rospy.logerr(msg)
388 
389  if consecutivescandropped > 3:
390  rospy.logerr("resetting device")
391  cleanup()
392  break
393 
394  del raw_data[:]
395  ser.write("n\n0\n") # stop broadcast, lidar disable
396  rospy.sleep(0.1)
397  ser.write("1\nb\n") # lidar enble, start broadcast
398  consecutivescandropped += 1
399  continue
400 
401  else:
402  consecutivescandropped = 0
403 
404 
405 
406  if scannum <= 4: # drop 1st few scans while lidar spins up
407  del raw_data[:]
408  continue
409 
410 
411  rebroadcastscan = None
412 
413  scan = LaserScan()
414 
415  scan.header.stamp = current_time - rospy.Duration(cycle) # rospycycle
416  lastscantime = current_time.to_sec() - cycle # seconds, floating point (used by rebroadcastscan only)
417 
418  scan.header.frame_id = frameID
419 
420  scan.angle_min = 0.0
421  rpmcycle = cycle
422  scan.angle_max = (readInterval*(count-1))/rpmcycle * 2 * math.pi
423  scan.angle_increment = (scan.angle_max-scan.angle_min) / (count-1)
424 
425  scan.time_increment = readInterval
426  scan.scan_time = readInterval * count # cycle # rospycycle.to_sec()
427 
428  scan.range_min = minimum_range
429  scan.range_max = maximum_range
430 
431  zeroes = 0
432  scan.ranges=[]
433  for x in range(len(raw_data)-(count*2)-headercodesize, len(raw_data)-headercodesize, 2):
434  low = ord(raw_data[x])
435  high = ord(raw_data[x+1])
436  value = ((high<<8)|low) / 100.0
437  scan.ranges.append(value)
438 
439  """ blank frame masks W/RHR+ degrees start, stop """
440  i = 0
441  while i < len(masks):
442  for x in range(int(count*((masks[i])/360.0)), int(count*((masks[i+1])/360.0)) ):
443  scan.ranges[x] = 0
444  i += 2
445 
446  """ if blanking scans when turning """
447  if dropscan:
448  for i in range(len(scan.ranges)):
449  scan.ranges[i] = 0
450  dropscan = False
451 
452  """ publish scan """
453  scan_pub.publish(scan)
454 
455  del raw_data[:]
456 
457 def rebroadcast():
458  global lastscantime, rebroadcastscan
459 
460  if lastscantime <= (rospy.get_time() - nominalcycle*2):
461  lastscantime += nominalcycle
462  rebroadcastscan.header.stamp = rospy.Time.from_sec(lastscantime)
463  scan_pub.publish(rebroadcastscan)
464 
465 
466 # main
467 
468 rospy.init_node('lidarbroadcast', anonymous=False)
469 rospy.on_shutdown(cleanup)
470 scan_pub = rospy.Publisher(rospy.get_param('~scan_topic', 'scan'), LaserScan, queue_size=3)
471 rospy.Subscriber("odom", Odometry, odomCallback) # TODO: make optional?
472 frameID = rospy.get_param('~frame_id', 'laser_frame')
473 
474 Server(XaxxonOpenLidarConfig, dynamicConfigCallback)
475 client = dynamic_reconfigure.client.Client("lidarbroadcast", timeout=30)
476 device = usbdiscover
477 
478 while True:
479  if ser == None and not rospy.is_shutdown(): #connect
480  ser = device.usbdiscover("<id::xaxxonopenlidar>", 5)
481 
482  ser.write("y\n") # get version
483  rospy.sleep(0.1)
484  while ser.inWaiting() > 0:
485  line = ser.readline().strip()
486  line = "firmware: "+line
487  rospy.loginfo(line)
488  ser.write("d"+chr(DIR)+"\n") # set direction (1=CW RHR+ motor@bottom default, ROS default)
489 
490  if enable:
491  updateRotateForwardOffset() # needs to be 1st
492  updateHeaderOffset()
493  updateRPM()
494  updateParkOffset()
495  updateReadInterval()
496  # rebroadcastscan = None
497 
498  readlidar(ser) # blocking
499 
500  else:
501  if lidarrunning:
502  ser.write("f\n") # stop lidar
503  lidarrunning = False
504  rospy.loginfo("SCAN: lidar disabled");
505  rebroadcastscan = scan
506  elif rebroadcastscan and BROADCASTLAST:
507  rebroadcast()
508 
509 
510  if rospy.is_shutdown():
511  break
512  else:
513  device.removelockfile()
514 
515  rospy.sleep(0.01)
516 
lidarbroadcast.readlidar
def readlidar(ser)
Definition: lidarbroadcast.py:276
lidarbroadcast.odomCallback
def odomCallback(data)
Definition: lidarbroadcast.py:65
lidarbroadcast.updateHeaderOffset
def updateHeaderOffset()
Definition: lidarbroadcast.py:126
lidarbroadcast.updateMasks
def updateMasks(string)
Definition: lidarbroadcast.py:159
lidarbroadcast.updateRotateForwardOffset
def updateRotateForwardOffset()
Definition: lidarbroadcast.py:236
lidarbroadcast.dynamicConfigCallback
def dynamicConfigCallback(config, level)
Definition: lidarbroadcast.py:93
lidarbroadcast.updateReadInterval
def updateReadInterval()
Definition: lidarbroadcast.py:216

xaxxon_openlidar
Author(s): Colin Adamson
autogenerated on Fri May 22 2020 04:05:28