sick_ldmrs.dataproc

49 """ 50 Class for processing and packaging Sick LD-MRS Laser Scan data messages into ROS messages. 51 Converts raw scan data into ROS PointCloud2 and LaserScan messages and publishes them on request. 52 The primary method you need to call is process_msg. This method processes the scan data 53 and publishes LaserScan and/or PointCloud2 messages of the (transformed) data. 54 You will also need to call set_timestamp_delta() to set the difference between the 55 current time and the LD-MRS on-board time (which you can extract from the message header) 56 """ 57 58 header_format = '<HHHQQHhhHhhhhhhH' # format string for data header 59 header_struct = Struct(header_format) # precompile for speed 60 # setup structure for pulling out header components 61 header_keys = ['ScanNumber', 'ScannerStatus', 62 'SyncPhaseOffset', 'ScanStartTimeNTP', 63 'ScanEndTimeNTP', 'AngleTicksPerRot', 64 'StartAngle', 'EndAngle', 65 'NumScanPoints', 'Reserved1', 66 'Reserved2', 'Reserved3', 67 'Reserved4', 'Reserved5', 68 'Reserved6', 'Reserved7'] 69 70 num_header_bytes = 44 # 44 bytes for the header 71 num_point_bytes = 10 # 10 bytes for each point 72 # lookup table for beam elevation trig factors 73 # taking centre of beam as elevation angle (actual limits are [-1.6, -0.8, 0, 0.8, 1.6]) 74 elev_angles = np.array([-1.2, -0.4, 0.4, 1.2]) * ((2*np.pi)/360.0) # rad 75 v_sin_lut = np.sin(elev_angles) 76 v_cos_lut = np.cos(elev_angles) 77

78 - def __init__(self, topics, params):

79 """ Constructor 80 @param topics: dictionary mapping topic names to publisher handles 81 valid topic names are {"cloud", "scan0", "scan1", "scan2", "scan3"} 82 @type topics: dict {topic_name:publisher_handle} 83 @param params: dictionary mapping parameter names to values. 84 Where applicable, the parameters must be in device units (e.g. ticks) 85 @type params: dict {ros_parameter_string:value} 86 """ 87 self.params = params 88 self.topics = topics 89 90 # LaserScan messages (numbered 0-3, 0 is the lowest scan plane) 91 # rewire for numpy serialization on publish 92 self.scans = [numpy_msg(LaserScan)(), 93 numpy_msg(LaserScan)(), 94 numpy_msg(LaserScan)(), 95 numpy_msg(LaserScan)()] 96 self._init_scans() 97 98 # PointCloud2 message 99 # rewire for numpy serialization on publish 100 self.point_cloud = numpy_msg(PointCloud2)() 101 self._init_point_cloud() 102 103 # put variables into the namespace to prevent 104 # attribute exceptions when the class is abused 105 self.header = None 106 self.x = None 107 self.y = None 108 self.z = None 109 self.echo = None 110 self.layer = None 111 self.flags = None 112 self.echo_w = None 113 self.h_angle = None 114 self.rads_per_tick = None 115 self.pc_data = None 116 self.last_start_time = None 117 self.rads_per_tick = None 118 self.seq_num = -1 119 self.time_delta = rospy.Duration() 120 self.n_points = 0 121 # timestamp smoothing 122 self.smoothtime = None 123 self.smoothtime_prev = None 124 self.recv_time_prev = None 125 self.known_delta_t = rospy.Duration(256.0/self.params['scan_frequency']) 126 self.time_smoothing_factor = self.params['time_smoothing_factor'] 127 self.time_error_threshold = self.params['time_error_threshold'] 128 self.total_err = 0.0 #integrate errors 129 self.header = {}.fromkeys(self.header_keys)

130

131 - def process_msg(self, msg, recv_time):

132 """ Process an incoming data message. Convert to (and publish) 133 PointCloud2 and LaserScan ROS messages depending 134 on whether the associated topics 135 ('cloud', 'scan0', 'scan1', 'scan2', 'scan3') resp. 136 are subscribed. 137 @param msg: the scan data message from the LD-MRS to be processed 138 @type msg: read-only byte buffer (e.g. a python string) 139 """ 140 # smooth the timestamp using the expected rate 141 self.smooth_timestamp(recv_time) 142 143 self.msg = msg 144 subscribers = self.num_subscribers() 145 146 # any subscribers? if not just return, don't waste cpu cycles 147 if any([n > 0 for n in subscribers.itervalues()]): 148 self.unpack_data(msg) 149 # is anyone subscribed to the cloud topic? 150 if subscribers["cloud"]: 151 self.make_point_cloud() 152 self.publish_point_cloud() 153 # is anyone subscribed to a scan topic? 154 if any(["scan" in topic and num_subs > 0 for topic, num_subs in subscribers.iteritems()]): 155 self.make_scans() 156 self.publish_scans() 157 158 return None

159 160

161 - def num_subscribers(self):

162 """ Get the number of subscribers for each ROS topic defined in self.topics 163 """ 164 subscribers = {} 165 for topic, handle in self.topics.iteritems(): 166 subscribers[topic] = handle.get_num_connections() 167 return subscribers

168 169

170 - def compute_tick_freq(self):

171 """ Compute the tick frequency from the start/end angles and times. 172 This is typically within 1% of the nominal values. 173 Note: we are using the device supplied tick resolution of 1/32nd degree 174 @return: tickfrequency - a floating point number (ticks per second) 175 """ 176 delta_t = (self.header['ScanEndTimeNTP'] - self.header['ScanStartTimeNTP']) 177 delta_ticks = self.header['StartAngle'] - self.header['EndAngle'] 178 tick_freq = (delta_ticks << 32)/ float(delta_t) 179 return int(tick_freq)

180 181

182 - def set_timestamp_delta(self, time_delta):

183 """ Set the time delta to apply to the LD-MRS timestamp to bring 184 it up to current ROS time. 185 @param time_delta: the time delta to be applied to the timestamps from the LD-MRS 186 @type time_delta: a rostime.Duration object 187 """ 188 self.time_delta = time_delta

189

190 - def smooth_timestamp(self, recv_time):

191 """ Smooth the timestamp to track the expected scan rate. 192 Parameter time_smoothing_factor controls 193 Small errors between rostime and smoothed time are corrected by 194 applying a correction weighted by the time_smoothong_gain 195 Large errors above the time_error_threshold are corrected to 196 recv_time by a step adjustment 197 @param recv_time: ros timestamp when the message was recieved 198 @type ldmrs_time: rostime.Time object 199 """ 200 201 if not self.smoothtime_prev: 202 # initialize to recv time 203 self.smoothtime = recv_time 204 else: 205 self.smoothtime = self.smoothtime_prev + self.known_delta_t 206 err = (recv_time - self.smoothtime).to_sec() 207 if self.time_smoothing_factor > 0 and abs(err) < self.time_error_threshold: 208 correction = rospy.Duration(err * (1-self.time_smoothing_factor)) 209 self.smoothtime += correction 210 else: 211 # error too high, or smoothing disabled - set smoothtime to last timestamp 212 self.smoothtime = recv_time 213 #print 'delta_smoothtime: %f, err: %f'%((self.smoothtime - self.smoothtime_prev).to_sec(), err) 214 self.smoothtime_prev = self.smoothtime

215 216

217 - def unpack_data(self, msg):

218 """ Unpack the data from an LD-MRS scan data message 219 into instance attributes to be picked up by make_point_cloud and/or 220 make_scan. 221 @param msg: a binary string in little-endian format encoding the 222 message 223 @type msg: read-only byte buffer (e.g. a python string) 224 """ 225 226 # increment ROS message header sequence number for this scan 227 # we are storing these internally rather than using the device supplied sequence number 228 # since the device seq number is only uint16 and will quickly roll over 229 #whereas the ROS message seq header field is uint32 230 self.seq_num += 1 231 232 # Parse the scan header into a dictionary 233 header_tuple = self.header_struct.unpack_from(msg) 234 for index, value in enumerate(header_tuple): 235 self.header[self.header_keys[index]] = value 236 237 self.rads_per_tick = (2.0 * np.pi) / self.header['AngleTicksPerRot'] 238 self.scan_start_time = self.smoothtime #util.NTP64_to_ROStime(self.header['ScanStartTimeNTP']) + self.time_delta 239 240 # check that the input string has the correct number of bytes 241 self.n_points = self.header['NumScanPoints'] 242 243 # Check we have enough bytes in the buffer 244 n_data_bytes = len(msg) - self.num_header_bytes 245 n_points_in_buffer = int(n_data_bytes/self.num_point_bytes) 246 if n_points_in_buffer != self.n_points: 247 rospy.logwarn("Number of point indicated by header (", self.n_points, 248 ") is more than number of points in buffer (", n_points_in_buffer, ")") 249 self.n_points = n_points_in_buffer 250 251 # tick frequency (Hz*32) 252 # Observed to vary by 1-2 percent from nominal frequency 253 self.tick_freq = self.compute_tick_freq() 254 255 if self.n_points is 0: 256 # No data to unpack 257 self.point_data = np.array([], dtype=uint16le) 258 else: 259 260 # Make a bytearray from the point data in the input string 261 uint16le = np.dtype(np.uint16, align='<') # force little endian 262 self.point_data = np.frombuffer(msg, dtype=uint16le, 263 count = self.n_points * self.num_point_bytes/2, #/2 accounts for 16bit view 264 offset = self.num_header_bytes) 265 266 # reshape array as 2D array of uint16 (n rows * num_point_bytes/2 cols) 267 self.point_data = np.reshape(self.point_data, [self.n_points, -1]) 268 269 # now array has format: 270 # Name: Layer/Echo/Flags | H Angle | Rdist | Echo Width | Reserved | 271 # Column: 0 | 1 | 2 | 3 | 4 | 272 273 # Pull out data as 16bit fields using slices and views 274 self.h_angle_ticks = self.point_data[:, 1].view(np.int16) 275 # adjust the start time to account for the tick shift 276 277 # compute number of scanner ticks since start angle tick for each sample 278 self.ticknum = -(self.h_angle_ticks - self.header['StartAngle']) 279 # radial distance to each point (in metres) 280 self.r_dist = (self.point_data[:, 2]/100.0).astype(np.float32) 281 # echo width (in cm!) 282 self.echo_w = self.point_data[:, 3].copy() 283 284 # Pull out echo layer and flags fields from echo_layer_flags 285 # need to copy so we can view as two byte arrays 286 layer_echo_flags = self.point_data.view(dtype=np.uint8) # view as 2D byte array 287 self.layer = layer_echo_flags[:,0].copy() 288 self.layer &= np.array([3], dtype=np.uint8) # extract bits 0-1 289 self.echo = layer_echo_flags[:,0].copy() 290 self.echo = (self.echo & np.array([48], dtype=np.uint8)) >> 4 # extract bits 4,5 and shift 291 292 # Valid Flags bits are 0,1,3 -- want to move bit 3 to bit 2 293 # so we can put layer,echo and flags into one byte for point cloud later 294 self.flags = layer_echo_flags[:,1].copy() 295 self.flags |= ((self.flags & np.array([8], dtype=np.uint16)) >> 1) # copy bit 3 to bit 2 296 self.flags &= np.array([7], dtype=np.uint8) # mask off bit 3 keeping bits 0-2

297 298

299 - def publish_point_cloud(self):

300 """ Publish the finished point cloud to the ROS network 301 topic is /<node_name>/cloud 302 """ 303 if self.point_cloud is not None: 304 handle = self.topics["cloud"] 305 handle.publish(self.point_cloud) 306 else: 307 rospy.logerr("Trying to publish empty point cloud to topic %s"%handle.name)

308 309

310 - def publish_scans(self):

311 """ Publish scan topics on the ROS network. 312 Publishes four topics (one for each scan plane of the LD-MRS): 313 /<node_name>/scan0 -- the lowest scan plane 314 /<node_name>/scan1 315 /<node_name>/scan2 316 /<node_name>/scan3 -- the highest scan plane 317 """ 318 for i, scan in enumerate(self.scans): 319 scan_id = "scan" + str(i) 320 handle = self.topics[scan_id] 321 if scan is not None and handle is not None: 322 handle.publish(scan) 323 else: 324 rospy.logerr("Trying to publish empty scan to topic %s"%handle.name)

325 326

327 - def make_point_cloud(self):

328 """Construct a point cloud from previously unpacked data (see unpack_data()) 329 The finished point cloud is stored as the instance variable 'point_cloud' 330 Note you must call unpack_data before invoking this method 331 """ 332 333 if self.n_points is 0: 334 # No points 335 self.pc_data = "" 336 else: 337 # compute x,y,z coordinates in metres 338 h_angle_rads = self.h_angle_ticks * self.rads_per_tick 339 v_sines = self.v_sin_lut[self.layer] # lookup cosines for elevation angle 340 v_cosines = self.v_cos_lut[self.layer] # lookup cosines for elevation angle 341 342 # x is in direction of travel 343 # z is up 344 # y is left 345 # note that the scan direction is clockwise from y-axis toward x-axis 346 # this is the reverse of the expected scan direction for this coordinate system. 347 # We account for this explicitly in the LaserScan messages (see _fill_laser_scans()) 348 self.x = (v_cosines * (np.cos(h_angle_rads) * self.r_dist)).astype(np.float32) 349 self.y = (v_cosines * (np.sin(h_angle_rads) * self.r_dist)).astype(np.float32) 350 self.z = (v_sines * self.r_dist).astype(np.float32) 351 352 # store delta from start time 353 self.time_deltas = (self.ticknum * 1.0/self.tick_freq).astype(np.float32) 354 355 # pack layer/echo/flags bytes into a single byte field 356 # Bit: 0,1, | 2,3 | 4,5,6 | 357 # Meaning: Layer | Echo | Flags | 358 self.layer_echo_flags = self.layer | (self.echo << 2) | (self.flags << 4) 359 360 # concatenate the numpy arrays in the correct order for the PointCloud2 message 361 # need to pack as 2D array, then flatten since fields have varying byte widths 362 data = np.hstack((self.x.view(np.uint8).T.reshape(-1, 4), 363 self.y.view(np.uint8).T.reshape(-1, 4), 364 self.z.view(np.uint8).T.reshape(-1, 4), 365 self.time_deltas.view(np.uint8).T.reshape(-1, 4), 366 self.echo_w.view(np.uint8).T.reshape(-1, 2), 367 self.layer_echo_flags.reshape(-1, 1))) # already uint8 368 data = data.reshape(-1) # 1D view with points correctly aligned 369 370 # convert to byte string for serialization and transmission 371 # The serialize_numpy method in _PointCloud2.py *should* (but doesn't) 372 # take care of this when the message is published; 373 # i.e. we should be able to leave this as a numpy array here. 374 # This is (probably) an oversight by the developer in this instance 375 self.pc_data = data.tostring() 376 377 # finished computing data, now fill out the fields in the message ready for transmission 378 self._fill_point_cloud()

379 380

381 - def make_scans(self):

382 """ Generate laser scan messages from previously unpacked data 383 the ROS parameter value of 'use_first_echo' in the ROS parameter 384 server determines if first or last echo is used 385 """ 386 387 if self.n_points is 0: 388 # slot ranges into their correct location within the scan array 389 # and separate out each layer using the layer breaks computed earlier 390 self.scan_data = np.zeros([4,0], dtype=np.float32) 391 392 else: 393 394 # number of points to allocate per scan message 395 # (depends on angular resolution and scan angle) 396 self.npoints_scan = abs((self.header['StartAngle'] - self.header['EndAngle'])/self.ticknum2ind) 397 398 # Now get indices of first and last echos... 399 # this is non-trivial since: 400 # 1. each point has 0-3 echoes (no guarantee about order is given) 401 # 2. points are interlaced by scan plane 402 # ... so we need to lexical sort by: echo, then tick number, then layer 403 # to get data in the right order. 404 # Next we need to compute the indices for the 405 # first/last echo for each sample using index differencing 406 407 # do lexical sort to preserve order of previous sorts. 408 # The echo sort may be redundant but SICK gives no guarantees on how 409 # the echoes are organized within the scan 410 sort_inds = np.lexsort((self.echo, self.ticknum, self.layer)) 411 # find where h_angle_ticks changes 412 ind_diff = np.diff(self.h_angle_ticks[sort_inds]) 413 breaks = ind_diff.nonzero()[0] # [0] due to singleton tuple return from nonzero() 414 415 if self.params[LDMRSParams.use_first_echo]: 416 # get indices of first echoes 417 # breaks +1 indexes the first echo of each sample 418 # prepend 0 since diff result has length n-1 and 419 # first sample is always the first echo 420 inds = np.hstack((np.array([0]), breaks + 1)) # sorted indices of first echoes 421 else: 422 # get indices of last echoes 423 # breaks indexes the last echo 424 # append last index since diff result has length n-1 and 425 # last sample is always the last echo 426 last_ind = np.array([self.r_dist.size-1]) 427 inds = np.hstack((breaks, last_ind)) #sorted indices of last echoes 428 429 # back out to unsorted inds so we can filter the required data 430 inds = sort_inds[inds] 431 432 #select first/last echo entries (elements remain sorted by ticknum and layer) 433 layers = self.layer[inds] 434 ranges = self.r_dist[inds] 435 ticks = self.ticknum[inds] 436 437 # Finally we need to find the layer breaks and store each layer separately 438 # in a zero padded array of the correct size. 439 440 # find the indices of the layer breaks 441 layer_breaks = np.diff(layers).nonzero()[0] 442 #index of first entry for each layer 443 firsts = np.hstack((np.array([0]), layer_breaks + 1)) 444 #index of last entry for each layer 445 lasts = np.hstack((layer_breaks, np.array([ranges.size -1]))) 446 447 # !!! NOTE: When scan frequency is 50 Hz this shifts the scan clockwise by 1/16th degree 448 # a better solution would be to move the start angle and start time correspondingly 449 tick_inds = np.round(((ticks - self.tick_ind_adjust)/self.ticknum2ind)).astype(np.int16) # integer division 450 451 # slot ranges into their correct location within the scan array 452 # and separate out each layer using the layer breaks computed earlier 453 self.scan_data = np.zeros((4, self.npoints_scan), dtype = np.float32) 454 455 for i in range(0, 4): 456 self.scan_data[i, tick_inds[firsts[i]:(lasts[i]+1)]] = ranges[firsts[i]:(lasts[i]+1)] 457 458 # finished marsahlling the range data, now it's time to fill in the details 459 self._fill_scans()

460 461 #-------------------------------------------------------------------------- 462 # Private methods 463

464 - def _init_point_cloud(self):

465 """ Set up the point cloud class as much as possible in advance 466 """ 467 468 pc = self.point_cloud 469 470 # set frame id (for the cloud topic this is just the frame_id_base) 471 pc.header.frame_id = self.params[LDMRSParams.frame_id_prefix] 472 473 # set up point fields 474 # there are 6 fields: 475 # x,y,z,timedelta,echowidth,layerechoflags 476 fields = [['x', PointField.FLOAT32, 0], 477 ['y', PointField.FLOAT32, 4], 478 ['z', PointField.FLOAT32, 8], 479 ['timedelta', PointField.FLOAT32, 12], 480 ['echowidth', PointField.UINT16, 16], 481 ['layerechoflags', PointField.UINT8, 18]] 482 483 # set up PointFields that describe the layout of the data 484 # blob in the message 485 point_fields = [] 486 for field in fields: 487 pf = PointField() 488 pf.name = field[0] 489 pf.datatype = field[1] 490 pf.offset = field[2] 491 pf.count = 1 492 point_fields.append(pf) 493 pc.fields = point_fields 494 495 # set up other attributes 496 pc.height = 1 # data is 1D 497 pc.is_dense = False # unordered points 498 pc.point_step = 19 # bytes per point 499 500 # endianness check and set 501 if struct.pack("h", 1) == "\000\001": 502 pc.is_bigendian = True 503 else: 504 pc.is_bigendian = False

505 506

507 - def _fill_point_cloud(self):

508 """ Pack the point cloud data into the PointCloud2 message """ 509 510 pc = self.point_cloud 511 512 # ------------------Header---------------------------- 513 pc.header.stamp = self.smoothtime 514 pc.header.seq = self.seq_num 515 516 # ----------------- Other ---------------------------- 517 num_points = len(self.x) 518 pc.width = num_points 519 pc.row_step = len(self.pc_data) # number of bytes of data 520 521 #------------------- Data ------------------- 522 pc.data = self.pc_data

523 524

525 - def _init_scans(self):

526 """ Initial set up for the Scan messages 527 """ 528 # conversion factors from tick num to scan index used in make_scans 529 tick_freq = self.params[LDMRSParams.scan_frequency] 530 self.ticknum2ind = tick_freq/800 # 4, 8, 16 which corresponds to 1/8, 1/4, and 1/2 degree for 12.5, 25, 50 Hz 531 532 # convert ticks to indices with a coarser resolution base 533 # to conserve space in the scan message. Original base is 1/32 degree. 534 # Otherwise there would be a lot of zero padding in between points 535 # and thus extra b/w overhead in the message 536 # angle_ticks has a 2 tick offset when scan frequency is 50Hz 537 # need to shift ticks down by 2 so we can divide ticks to get scan indices later 538 if self.params['scan_frequency'] == 12800: # 50Hz 539 self.tick_ind_adjust = 2 540 time_per_tick = 1.0/(50.0 * 11520) 541 self.start_time_adjust = rospy.Duration(2 * time_per_tick) 542 else: 543 self.tick_ind_adjust = 0 544 self.start_time_adjust = rospy.Duration(0) 545 546 frame_id_prefix = self.params[LDMRSParams.frame_id_prefix] 547 548 for i, scan in enumerate(self.scans): 549 scan.header.frame_id = frame_id_prefix + str(i) # append the scan plane number to the frame id 550 scan.intensities = np.array([]) # empty (must be numpy array for serialization) 551 scan.range_min = 0.01 # using 1cm so that zero range points are not displayed in RVIZ 552 scan.range_max = 10000 # No max range specified on spec sheet

553 554

555 - def _fill_scans(self):

556 """Add scan data to the scans and set up per-message parameter values 557 """ 558 # adjust start time and start angle if using 50 Hz to compensate 559 # for 2 sample offset of first sample 560 #tick2rad = 2*np.pi/11520 561 #start_angle_adjust = self.tick_ind_adjust*tick2rad 562 #start_time_adjust = self.tick_ind_adjust/12800.0 563 #start_time = self.scan_start_time + rospy.Duration(start_time_adjust) 564 565 # compute the time since the last scan started 566 if self.smoothtime_prev is None: 567 time_between_scans = 0.0 568 else: 569 time_between_scans = self.smoothtime - self.smoothtime_prev - self.start_time_adjust 570 time_between_scans = time_between_scans.secs + time_between_scans.nsecs/1e9 571 572 # Set up angles 573 # NOTE!: the scanner rotates clockwise from above so 574 # angle_min > angle_max and we use a NEGATIVE ANGLE INCREMENT 575 # this works in RVIZ so we are going with it 576 # the problem stems from the poorly named angle_min and angle_max fields in LaserScan. 577 # These should probably have been named angle_start and angle_end (or similar) to prevent 578 # assumptions about which one is greater, and hence assumptions about 579 # the sign of the angle_increment. 580 angle_min = (self.header['StartAngle'] - self.tick_ind_adjust)*self.rads_per_tick # rad 581 angle_max = (self.header['EndAngle'] - self.tick_ind_adjust)*self.rads_per_tick # rad 582 angle_increment = -self.ticknum2ind*self.rads_per_tick # angle between samples (rad) 583 584 # Times are handled as you would expect (time increment is +ve): 585 # time for each point = header.stamp + time_increment*<index> 586 time_increment = float(self.ticknum2ind)/self.tick_freq #time between samples 587 588 # set up each scan (only the range data differs) 589 for i, scan in enumerate(self.scans): 590 scan.header.stamp = self.scan_start_time + self.start_time_adjust 591 scan.header.seq = self.seq_num 592 scan.scan_time = time_between_scans 593 scan.angle_min = angle_min 594 scan.angle_max = angle_max 595 scan.angle_increment = angle_increment 596 scan.time_increment = time_increment 597 scan.ranges = self.scan_data[i, :] 598 599 # store the current time to generate the next scan.scan_time 600 self.last_start_time = self.scan_start_time

Source Code for Module sick_ldmrs.dataproc