def data_gen(*args):
fret = llt.get_actual_profile(hLLT, profile_buffer, len(profile_buffer), llt.TProfileConfig.PARTIAL_PROFILE,
ct.byref(lost_profiles))
if fret != len(profile_buffer):
print("Error get profile buffer data: " + str(ret))
fret = llt.convert_part_profile_2_values(profile_buffer, len(profile_buffer), ct.byref(partial_profile_struct), scanner_type, 0,
null_ptr_short, null_ptr_short, null_ptr_short, x_p, z_p, null_ptr_int, null_ptr_int)
if fret & llt.CONVERT_X is 0 or fret & llt.CONVERT_Z is 0:
raise ValueError("Error converting data: " + str(ret))
for i in range(16):
timestamp[i] = profile_buffer[resolution * data_width - 16 + i]
llt.timestamp_2_time_and_count(timestamp, ct.byref(shutter_opened), ct.byref(shutter_closed), ct.byref(profile_count), null_ptr_short)
# Adjust ROI to area with circle
x_roi = x[(x > lower_x_limit) & (x < upper_x_limit) & (z > lower_z_limit) & (z < upper_z_limit)]
z_roi = z[(x > lower_x_limit) & (x < upper_x_limit) & (z > lower_z_limit) & (z < upper_z_limit)]
# Fitting
xc, zc, r, residuals = leastsq_circle(x_roi, z_roi)
theta_fit = np.linspace(-np.pi, np.pi, 90)
print(r)
print(xc, zc)
# Prepare draw data
x_fit = xc + r*np.cos(theta_fit)
z_fit = zc + r*np.sin(theta_fit)
time.sleep(0.1)
# yield iterator
yield x, z, xc, zc, x_fit, z_fit
def data_gen(*args):
event.wait()
fret = llt.convert_part_profile_2_values(profile_buffer,
len(profile_buffer),
ct.byref(partial_profile_struct),
scanner_type, 0, null_ptr_short,
null_ptr_short, null_ptr_short,
x_p, z_p, null_ptr_int,
null_ptr_int)
if fret & llt.CONVERT_X is 0 or fret & llt.CONVERT_Z is 0:
raise ValueError("Error converting data: " + str(ret))
for i in range(16):
timestamp[i] = profile_buffer[resolution * data_width - 16 + i]
llt.timestamp_2_time_and_count(timestamp, ct.byref(shutter_opened),
ct.byref(shutter_closed),
ct.byref(profile_count), null_ptr_short)
event.clear()
yield x, z
def measure(self, data, size):
"""Trigger one measurement
"""
profile_buffer = (ct.c_ubyte * (self.resolution * 64))()
ct.memmove(profile_buffer, data, size)
shutter_opened = ct.c_double(0.0)
shutter_closed = ct.c_double(0.0)
profile_count = ct.c_uint(0)
# Declare measuring data array
x = (ct.c_double * self.resolution)()
z = (ct.c_double * self.resolution)()
intensities = (ct.c_ushort * self.resolution)()
# Null pointer if data not necessary
null_ptr_short = ct.POINTER(ct.c_ushort)()
null_ptr_int = ct.POINTER(ct.c_uint)()
timestamp = (ct.c_ubyte * 16)()
try:
ret = llt.convert_profile_2_values(
profile_buffer, len(profile_buffer), self.resolution,
llt.TProfileConfig.PROFILE, self.scanner_type, 0,
null_ptr_short, intensities, null_ptr_short, x, z,
null_ptr_int, null_ptr_int)
if ret & llt.CONVERT_X is 0 or ret & llt.CONVERT_Z is 0 or \
ret & llt.CONVERT_MAXIMUM is 0:
raise ValueError("Error converting data: " + str(ret))
for i in range(16):
timestamp[i] = profile_buffer[self.resolution * 64 - 16 + i]
llt.timestamp_2_time_and_count(timestamp, ct.byref(shutter_opened),
ct.byref(shutter_closed),
ct.byref(profile_count),
null_ptr_short)
pc = PointCloud()
pc.header = Header()
pc.header.stamp = rospy.Time(0)
pc.header.frame_id = uEpsilon_Driver.TCP
# Create intensity channel
channel = ChannelFloat32()
channel.name = "Intensity"
for i in range(self.resolution):
pc.points.append(Point(x=float(x[i]), z=-float(z[i])))
channel.values.append(intensities[i])
pc.channels.append(channel)
gen = pc2.create_cloud(pc.header, FIELDS,
[(p.x, p.y, p.z) for p in pc.points])
self.reading_publisher_pcl2.publish(gen)
self.reading_publisher_pcl.publish(pc)
except Exception as e:
status(self.name, str(e), STATE.ERROR)
if ret != len(profile_buffer):
raise ValueError("Error get profile buffer data: " + str(ret))
ret = llt.convert_profile_2_values(profile_buffer, len(profile_buffer),
resolution, llt.TProfileConfig.PROFILE,
scanner_type, 0, null_ptr_short,
intensities, null_ptr_short, x, z,
null_ptr_int, null_ptr_int)
if ret & llt.CONVERT_X is 0 or ret & llt.CONVERT_Z is 0 or ret & llt.CONVERT_MAXIMUM is 0:
raise ValueError("Error converting data: " + str(ret))
for i in range(16):
timestamp[i] = profile_buffer[resolution * 64 - 16 + i]
llt.timestamp_2_time_and_count(timestamp, ct.byref(shutter_opened),
ct.byref(shutter_closed),
ct.byref(profile_count), null_ptr_short)
# Stop transmission
ret = llt.transfer_profiles(hLLT, llt.TTransferProfileType.NORMAL_TRANSFER, 0)
if ret < 1:
raise ValueError("Error stopping transfer profiles: " + str(ret))
# Disconnect
ret = llt.disconnect(hLLT)
if ret < 1:
raise ConnectionAbortedError("Error while disconnect: " + str(ret))
# Delete
ret = llt.del_device(hLLT)
if ret < 1: