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()
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 complete 3d scan
:param angular_range: the angular range for the base to cover during the scan (default 180)
"""
# Calcualte the # of evenly spaced 2D sweeps (base movements) required
# to match resolutions
num_sweeps = int(
round(self.settings.get_resolution() *
self.settings.get_scan_range()))
# Caclulate the number of stepper steps covering the angular range
num_stepper_steps = self.settings.get_scan_range(
) * self.base.get_steps_per_deg()
# Calculate number of stepper steps per move (ie: between scans)
num_stepper_steps_per_move = int(round(num_stepper_steps / num_sweeps))
# Actual angle per move (between individual 2D scans)
angle_between_sweeps = 1.0 * num_stepper_steps_per_move / \
self.base.get_steps_per_deg()
# correct the num_sweeps to account for the accumulated difference due
# to rounding
num_sweeps = math.floor(1.0 * self.settings.get_scan_range() /
angle_between_sweeps)
output_json_message({
'type':
"update",
'status':
"scan",
'msg':
"Initiating scan...",
'duration':
num_sweeps / self.settings.get_motor_speed(),
'remaining':
num_sweeps / self.settings.get_motor_speed()
})
# Start Scanning
self.device.start_scanning()
# put a 3 second timeout on the get_scans() method in case it gets hung
# up
time_out_thread = threading.Timer(3, self.check_get_scan_timeout)
time_out_thread.start()
# get_scans is coroutine-based generator lazily returning scans ad
# infinitum
for scan_count, scan in enumerate(self.device.get_scans()):
# 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 len(scan.samples) > self.settings.get_max_samples_per_scan():
continue
# Export the scan
self.exporter.export_2D_scan(
scan,
scan_count,
self.settings.get_mount_angle(),
# base angle before move
angle_between_sweeps * scan_count,
# base angle after move
angle_between_sweeps * (scan_count + 1),
False)
# Wait for the device to reach the threshold angle for movement
time.sleep(self.settings.get_time_to_deadzone_sec())
# Move the base
self.base.move_steps(num_stepper_steps_per_move)
output_json_message({
'type':
"update",
'status':
"scan",
'msg':
"Scan in Progress...",
'duration':
num_sweeps / self.settings.get_motor_speed(),
'remaining':
(num_sweeps - scan_count) / self.settings.get_motor_speed()
})
# Collect the appropriate number of 2D scans
if scan_count == num_sweeps:
break
# Stop scanning
self.device.stop_scanning()
output_json_message({
'type': "update",
'status': "complete",
'msg': "Finished scan!"
})