def perform_scan(self): """Performs a 3d scan""" # Calcualte some intermediate values #num_sweeps, angle_between_sweeps, steps_per_move = self.calculate_scan_variables() # Report that the scan is initiating, and start scanning #self.report_scan_initiated(num_sweeps) self.device.start_scanning() # put a 3 second timeout on the get_scans() method in case it hangs time_out_thread = threading.Timer(5, self.check_get_scan_timeout) time_out_thread.start() valid_scan_index = 0 rotated_already = False #if not CCW: #angle_between_sweeps = -angle_between_sweeps # get_scans is coroutine-based generator returning scans ad infinitum for scan_count, scan in enumerate(self.device.get_scans()): Cloud_Points = [] # note the arrival time scan_arrival_time = time.time() HEADER.stamp = rospy.Time.now() # note that a scan was received (used to avoid the timeout) self.received_scan = True # remove readings from unreliable distances scan_utils.remove_distance_extremes( scan, self.settings.get_min_range_val(), self.settings.get_max_range_val()) # Remove readings from the deadzone scan_utils.remove_angular_window( scan, self.settings.get_deadzone(), 360 - self.settings.get_deadzone()) #if valid_scan_index >= num_sweeps - 2: # Avoid redundant data in last few partially overlapping scans # Catch scans that contain unordered samples and discard them # (this may indicate problem reading sync byte) if scan_utils.contains_unordered_samples(scan): continue # Edge case (discard 1st scan without base movement and move base) if not rotated_already: # Wait for the device to reach the threshold angle for movement self.wait_until_deadzone(scan_arrival_time) # Move the base and start again #self.base.move_steps(steps_per_move) rotated_already = True continue # Export the scan #self.exporter.export_2D_scan( # scan, valid_scan_index, angle_between_sweeps, # self.settings.get_mount_angle(), False) #if not CCW: #angle_between_sweeps = -angle_between_sweeps converted_coords = scan_utils.transform_scan_without_base( scan, self.settings.mount_angle, 0.0) for n, sample in enumerate(scan.samples): world_x = converted_coords[n, 0] world_y = converted_coords[n, 1] world_z = converted_coords[n, 2] self.exporter.writer.writerow({ 'SCAN_INDEX': valid_scan_index, 'X': int(round(world_x)), 'Y': int(round(world_y)), 'Z': int(round(world_z)), 'SIGNAL_STRENGTH': sample.signal_strength }) #print "the samples!" row = [] row.append(float(world_x) / 100) row.append(float(world_y) / 100) row.append(float(world_z) / 100) row.append(sample.signal_strength) Cloud_Points.append(row) # increment the scan index valid_scan_index = valid_scan_index + 1 print "length of samples is %d" % len(scan.samples) # Wait for the device to reach the threshold angle for movement if len(scan.samples) > 0: point_cloud2 = pcl2.create_cloud(HEADER, FIELDS, Cloud_Points) #publish the point cloud message via ros self.cloud_publisher.publish(point_cloud2) # Move the base and report progress self.report_scan_progress(valid_scan_index) self.wait_until_deadzone(scan_arrival_time) # Move the base and report progress #self.base.move_steps(steps_per_move) self.report_scan_progress(valid_scan_index) # Exit after collecting the required number of 2D scans #if valid_scan_index >= num_sweeps: # break # Stop scanning and report completion #time.sleep(1.0) #self.device.stop_scanning() self.report_scan_complete()
def perform_scan(self): """Performs a 3d scan""" # Calculate some intermediate values num_sweeps, angle_between_sweeps, steps_per_move = self.calculate_scan_variables( ) # Report that the scan is initiating, and start scanning self.report_scan_initiated(num_sweeps) self.device.start_scanning() # put a 3 second timeout on the get_scans() method in case it hangs time_out_thread = threading.Timer(3, self.check_get_scan_timeout) time_out_thread.start() valid_scan_index = 0 rotated_already = False # get_scans is coroutine-based generator returning scans ad infinitum for scan_count, scan in enumerate(self.device.get_scans()): # note the arrival time scan_arrival_time = time.time() # note that a scan was received (used to avoid the timeout) self.received_scan = True # remove readings from unreliable distances scan_utils.remove_distance_extremes( scan, self.settings.get_min_range_val(), self.settings.get_max_range_val()) # Remove readings from the deadzone scan_utils.remove_angular_window( scan, self.settings.get_deadzone(), 360 - self.settings.get_deadzone()) if valid_scan_index >= num_sweeps - 2: # Avoid redundant data in last few partially overlapping scans scan_utils.remove_angular_window(scan, self.settings.get_deadzone(), 361) # Catch scans that contain unordered samples and discard them # (this may indicate problem reading sync byte) if scan_utils.contains_unordered_samples(scan): continue # Edge case (discard 1st scan without base movement and move base) if not rotated_already: # Wait for the device to reach the threshold angle for movement self.wait_until_deadzone(scan_arrival_time) # Move the base and start again self.base.move_steps(steps_per_move) rotated_already = True continue # Export the scan self.exporter.export_2D_scan(scan, valid_scan_index, angle_between_sweeps, self.settings.get_mount_angle(), False) # increment the scan index valid_scan_index = valid_scan_index + 1 # Wait for the device to reach the threshold angle for movement self.wait_until_deadzone(scan_arrival_time) # Move the base and report progress self.base.move_steps(steps_per_move) self.report_scan_progress(num_sweeps, valid_scan_index) # Exit after collecting the required number of 2D scans if valid_scan_index >= num_sweeps: break # Stop scanning and report completion self.device.stop_scanning() self.report_scan_complete()