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()