def example(args):
""" Example function for synchronizing several devices.
Call this script with a list of hosts (the IP addresses or the hostnames of the devices).
Some other parameters can be changed: frame_rate, vertical_fov, scan lines, pulse_mode, ntp_server_ip.
To get help, call the script with -h.
:param args: argparser will parse incoming arguments (hosts, frame_rate, vertical_fov, scan lines, pulse_mode, ntp_server_ip)
"""
devices = [blickfeld_scanner.scanner(host)
for host in args.hosts] # Connect to the device
config = scan_pattern_pb2.ScanPattern(
) # Create a scan pattern, see :any:`protobuf_protocol` ScanPattern
if args.vertical_fov:
config.vertical.fov = args.vertical_fov / 180 * math.pi # Set vertical FoV in rad
config.vertical.scanlines_up = args.scanlines[0] # Set up scan lines
config.vertical.scanlines_down = args.scanlines[1] # Set down scan lines
config.pulse.frame_mode = scan_pattern_pb2.ScanPattern.Pulse.FrameMode.Value(
args.pulse_mode) # Set pulse mode
if args.ntp_server_ip != "": # If the ntp_server_ip variable is set, set the ntp servers
for device in devices: # iterate through devices
device.set_time_synchronization(ntp=[args.ntp_server_ip])
print("Requested pattern:", config)
print(devices[0].fill_scan_pattern(config))
blickfeld_scanner.scanner.sync(devices=devices,
scan_pattern=config,
target_frame_rate=args.frame_rate)
def configure_point_cloud_stream(args):
"""Fetch a basic point cloud. Only get specific fields of each point: X, Y, Z, intensity and ID.
Use a filter to filter out points which are closer than 5m or further away than 50m.
:param args: arguments to parse out the hostname or IP address of the device and an optional file path to record to.
"""
device = blickfeld_scanner.scanner(args.target) # Connect to the device
# Reference Frame with X, Y, Z coordinates, intensity and point IDs
# The frame represents the desired data. To request a field, set it to any value (also in submessages). For a repeated field, add at least one element.
reference_frame = point_cloud.REF_FRAME_XYZ_I_ID
# Create filter to filter points and returns by point attributes during the post-processing on the device.
point_filter = scan_pattern_pb2.ScanPattern().Filter()
point_filter.range.minimum = 5 # minimum range for a point to be sent out is 5m
point_filter.range.maximum = 50 # maximum range for a point to be sent out is 50m
point_filter.max_number_of_returns_per_point = 2 # Set max number of returns to 2. The default value is 1.
stream = device.get_point_cloud_stream(
filter=point_filter,
reference_frame=reference_frame) # Create a point cloud stream object
while True:
# Receive frame. It has a reduced data size, as not all fields are requested. This also speeds up the Protobuf encoding & decoding.
frame = stream.recv_frame()
# Format of frame is described in protocol/blickfeld/data/frame.proto or doc/protocol.md
# Protobuf API is described in https://developers.google.com/protocol-buffers/docs/pythontutorial
print(f"Got {frame}")
def example(args):
""" Example to for handling protocol exceptions.
The errors are described in the protobuf protocol definition of the errors.
This example tries to set an invalid ScanPattern and a protocol exception will be raised and catched.
:param args: arguments to parse out the host name or IP address of the device
"""
device = blickfeld_scanner.scanner(args.host) # Connect to the device
config = scan_pattern_pb2.ScanPattern(
) # Create a scan pattern, see protobuf protocol definition of the scan pattern
config.horizontal.fov = np.deg2rad(10)
config.vertical.fov = np.deg2rad(30)
config.vertical.scanlines_up = 10 # Set invalid values
config.vertical.scanlines_down = 30
config.pulse.frame_mode = scan_pattern_pb2.ScanPattern.Pulse.FrameMode.Value(
"COMBINE_UP_DOWN")
try:
device.fill_scan_pattern(
config) # Try to fill the remaining fields with default values
except protocol_exception as pe: # Catch protocol exception and print it
print(pe)
try:
device.set_scan_pattern(config) # Send scan pattern to device
except protocol_exception as pe: # Catch protocol exception and print it
print(pe)
def set_algorithms(target, persist, background_subtraction, neighbor_filter):
"""Sets post-processing algorithms on the device.
This basic example just activates the algorithm with their default parameter set.
"""
device = blickfeld_scanner.scanner(args.target) # Connect to the device
device.set_default_point_cloud_algorithms(
persist=persist,
background_subtraction=background_subtraction,
neighbor_filter=neighbor_filter,
)
print("Applied algorithms in advanced configuration")
print(device.get_advanced_config().server.default_point_cloud_subscription.
algorithms)
print("Start a device stream to see the algorithms in action")
def get_frame(host: str):
""" Fetches a pointcloud frame of a device and dump the sample.
See "Getting Started" example in the Blickfeld-Scanner-Library documentation for a in depth explanation.
:param host: the host name or IP address of the device
:type host: str
"""
scanner = blickfeld_scanner.scanner(args.host) # Connect to the device
print("connected to device, starting stream...")
stream = scanner.get_point_cloud_stream(
filter=None) # Create a verbose point cloud stream object
stream.record_to_file("frame.bfpc") # Record the frame(s) to file
frame = stream.recv_frame() # Receive a frame
print("got measurements, closing stream...")
stream.close() # close immediately, we only need one frame
return frame
def custom_trigger(target, horizontal_angle):
"""Configure custom triggers and retrieve a point cloud for the trigger set.
This example will stop after 10 received frames.
:param target: hostname or IP address of the device
:param horizontal_angle: desired trigger angle per scanline in radians
"""
scanner = blickfeld_scanner.scanner(target)
# retrieve currently active scan pattern
pattern = scanner.get_scan_pattern()
# set pulse type to custom; we are goign to overwrite it
pattern.pulse.type = pattern.pulse.CUSTOM
custom_pattern = pattern.pulse.custom
# define a single trigger angle
at = scan_pattern_pb2.ScanPattern.Pulse.Custom.AngleTrigger.Angle()
# set the desired horizontal trigger angle (per scanline)
at.horizontal_angle = horizontal_angle
# set the desired trigger signals we want to have. TRG_LASER for regular acquisition, and TRG_EXTERNAL_0 for a GPIO output (not available with regular Cubes)
at.enabled_triggers.extend([
scan_pattern_pb2.ScanPattern.Pulse.Custom.TRG_LASER,
scan_pattern_pb2.ScanPattern.Pulse.Custom.TRG_EXTERNAL_0
])
# reset currently configured trigger angles
del custom_pattern.angle_trigger.angles[:]
# and add our single trigger to trigger list
custom_pattern.angle_trigger.angles.extend([at])
print("Configuration set send to device:")
print(custom_pattern)
scanner.set_scan_pattern(pattern)
# starting stream
stream = scanner.get_point_cloud_stream()
for i in range(10):
frame = stream.recv_frame()
print(f"Got {frame}")
stream.stop()
def example(args):
""" Example function for activating time synchronization on devices.
You can choose NTP or PTP synchronization and as an example two different parameters for each are set.
In the end the status of the synchronization will be read.
:param args: argparser will parse incoming arguments (host, ptp, ntp)
"""
device = blickfeld_scanner.scanner(args.host) # Connect to the device
print("Starting time_synchronization..")
device.set_time_synchronization(
ntp=args.ntp, ptp=args.ptp
) # Set time synchronization method. A list of ntp server or a list of ptp unicast destinations can be given as arguments as well.
print(device.get_status().time_synchronization
) # Print time synchronization status
def fetch_point_cloud(target):
"""Fetch the point cloud of a device and print the frame ID and number of returns in the received frame.
This example will stop after 10 received frames.
:param target: hostname or IP address of the device
"""
device = blickfeld_scanner.scanner(target) # Connect to the device
stream = device.get_point_cloud_stream(
) # Create a point cloud stream object
for i in range(10):
frame = stream.recv_frame(
) # Receive a frame. This frame will also be dumped into the file
# Format of frame is described in protocol/blickfeld/data/frame.proto or doc/protocol.md
# Protobuf API is described in https://developers.google.com/protocol-buffers/docs/pythontutorial
print(f"Got {frame}")
stream.stop()
def record_point_cloud(target, file_name):
"""Record a raw point cloud and print the number of received bytes.
If the record_to_file parameter is set with a path and a filename, the stream will be recorded to this file.
:param target: hostname or IP address of the device
:param file_name: file path to record to
"""
device = blickfeld_scanner.scanner(args.target) # Connect to the device
raw_stream = device.record_point_cloud_stream(
file_name) # Create a raw point cloud stream object
while True:
raw_file = raw_stream.recv_bytes() # Receive a raw file bytes
print(f"Got {len(raw_file)} bytes")
raw_file = raw_stream.stop()
print(f"Got {len(raw_file)} bytes and stopped")
def store_and_set_named_scan_pattern(target):
"""Get a list of named scan patterns of a device and print them.
Afterwards create a new scan pattern and store it with a name.
At last get the named scan patterns again and set the created scan pattern.
:param target: hostname or IP address of the device
"""
device = blickfeld_scanner.scanner(target) # Connect to the device
named_scan_patterns = device.get_named_scan_patterns() # Get named scan patterns
# Print if the scan pattern is a default or custom named scan pattern
for scan_pattern in named_scan_patterns.configs:
if scan_pattern.read_only:
print("'" + str(scan_pattern.name) + "' is a default named scan pattern.")
else:
print("'" + str(scan_pattern.name) + "' is a custom named scan pattern.")
# Set default scan pattern by name
device.set_scan_pattern(name="Default")
# Create new scan pattern
new_scan_pattern = scan_pattern_pb2.ScanPattern()
new_scan_pattern.vertical.scanlines_up = 100
new_scan_pattern.vertical.scanlines_down = 100
new_scan_pattern.pulse.frame_mode = scan_pattern_pb2.ScanPattern.Pulse.FrameMode.COMBINE_UP_DOWN
# Fill scan pattern to add missing parameter (optional)
new_scan_pattern = device.fill_scan_pattern(new_scan_pattern)
# Store scan pattern as "python test"
# This method persists the scan pattern on the device. Use 'device.delete_named_scan_pattern("python-test")' to delete it again.
device.store_named_scan_pattern("python-test", new_scan_pattern)
# Check if the stored scan pattern was correctly set (optional, just for visualisation)
named_scan_patterns = device.get_named_scan_patterns() # Get named scan patterns
for scan_pattern in named_scan_patterns.configs:
if scan_pattern.name == "python-test":
print("'" + str(scan_pattern.name) + "' succesfully stored.")
# Set scan pattern by name
device.set_scan_pattern(name="python-test")
def fetch_imu(target):
"""Fetch the IMU data and print it.
This example will stop after 10 received IMU bursts.
:param target: hostname or IP address of the device
"""
device = blickfeld_scanner.scanner(target) # Connect to the device
stream = device.get_imu_stream(
as_numpy=True) # Create a point cloud stream object
for i in range(10):
header, data = stream.recv_burst_as_numpy(
) # Receive an IMU data burst.
# Format of frame is described in protocol/blickfeld/data/frame.proto or doc/protocol.md
# Protobuf API is described in https://developers.google.com/protocol-buffers/docs/pythontutorial
print(f"Got burst: {header} with {data.dtype}:\n{data}")
rots = stream.convert_numpy_burst_to_rotations(data)
print(f" to rotations {rots.dtype}:\n{rots}")
stream.stop()
def fetch_numpy_frame(target):
"""Fetch the point cloud of a device as a numpy structued array.
This example will stop after 10 received frames.
:param target: hostname or IP address of the device
"""
device = blickfeld_scanner.scanner(target) # Connect to the device
# Create filter to filter points and returns by point attributes during the post-processing on the device.
point_filter = scan_pattern_pb2.ScanPattern().Filter()
point_filter.max_number_of_returns_per_point = 2 # Set max number of returns to 2. The default value is 1.
point_filter.delete_points_without_returns = True # Filter points with no returns. This reduces the dataset only to valid returns.
# Create a point cloud stream object
# The `as_numpy` flag enables the numpy support.
stream = device.get_point_cloud_stream(point_filter=point_filter,
as_numpy=True)
for i in range(10):
frame, data = stream.recv_frame_as_numpy()
print("Frame", str(frame.id) + ":", data)
print("Size:", data.size, "Type:", data.dtype)
# Example for filtering the strucuted array
multiple_returns = data[np.argwhere(data['return_id'] > 0)]
print("Got %d multiple returns" % (len(multiple_returns)))
# Extract cartesian coordinates
x, y, z = (data['cartesian']['x'], data['cartesian']['y'],
data['cartesian']['z'])
print(x, y, z)
print() # newline
stream.stop()
def calibrate_accelerometer(args):
"""Calibrate the rotational offset of the Blickfeld Cube 1 Inertial Measurement Unit (IMU).
The upright pose is identified by the static acceleration reading [0, 0, -1]. This means, that
the gravitational acceleration is measured along the negative direction of the devices Z-Axis.
Place the Blickfeld Cube 1 on a level surface for calibrating the IMU.
Avoid any kind of movement of the Blickfeld Cube 1 while running the script.
If the Blickfeld Cube 1 has already configured a rotational offset remove it first by running
this script with the '--remove' flag.
"""
ORIENTATION_UPRIGHT = [0, 0, -1]
ERROR_ALLOWED_NORM = 1e-2
# ensure a given vector is normalized to length 1
def _unit_vector(v: list) -> np.array:
return np.array(v) / np.linalg.norm(v)
# calculate the rotation matrix
def _calculate_rotation_matrix(acc_imu: list,
acc_calib: list = ORIENTATION_UPRIGHT
) -> np.array:
acc_imu = _unit_vector(acc_imu)
acc_calib = _unit_vector(acc_calib)
# see https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
imu_static_rotation_offset = np.eye(3)
if np.linalg.norm(np.cross(acc_calib, acc_imu)) < 1e-6:
imu_static_rotation_offset = -imu_static_rotation_offset
else:
axis = np.cross(acc_calib, acc_imu)
s = np.linalg.norm(axis)
c = np.dot(acc_calib, acc_imu)
axis_cross = np.zeros(9)
np.put(axis_cross, [1, 2, 3, 5, 6, 7],
[-axis[2], axis[1], axis[2], -axis[0], -axis[1], axis[0]])
axis_cross = axis_cross.reshape(3, 3)
imu_static_rotation_offset = np.eye(3) + axis_cross + np.dot(
axis_cross, axis_cross) * (1 - c) / (s**2)
return imu_static_rotation_offset
device = scanner(args.target)
print(f"connected to {args.target}")
cfg = device.get_advanced_config()
# clear imu_static_rotation_offset and exit
if args.remove:
del cfg.processing.imu_static_rotation_offset[:]
device.set_advanced_config(cfg, persist=args.persist)
print("static rotation offset removed")
exit(0)
# check for configured imu_static_rotation_offset
if cfg.HasField("processing") and len(
cfg.processing.imu_static_rotation_offset) != 0:
print("imu_static_rotation_offset is already configured")
print("remove configuration by starting this script with '--remove'")
exit(0)
# measure the actual static acceleration by removing configured imu_static_rotation_offset
del cfg.processing.imu_static_rotation_offset[:]
device.set_advanced_config(cfg)
sleep(0.3)
# calculate and set imu_static_rotation_offset
imu_static_rotation_offset = _calculate_rotation_matrix(
acc_imu=list(device.get_status().imu.static_state.acceleration))
cfg_new = Advanced()
cfg_new.MergeFrom(cfg)
cfg_new.processing.imu_static_rotation_offset[:] = imu_static_rotation_offset.flatten(
)
device.set_advanced_config(cfg_new)
sleep(0.3)
# check error after calibration
state = device.get_status()
acc_imu = _unit_vector(state.imu.static_state.acceleration)
print(
f"offset after calibration: {np.linalg.norm(ORIENTATION_UPRIGHT - acc_imu)} [g]"
)
# rollback in case error is too large (e.g. device was moved during calibration)
if np.linalg.norm(ORIENTATION_UPRIGHT - acc_imu) > ERROR_ALLOWED_NORM:
print(f"error too large, maximum allowed is {ERROR_ALLOWED_NORM}")
print("rolling back changes")
del cfg.processing.imu_static_rotation_offset[:]
device.set_advanced_config(cfg)
exit(0)
device.set_advanced_config(cfg_new, persist=args.persist)
def test_record_point_cloud_dump(ip_or_hostname="localhost"):
scanner = blickfeld_scanner.scanner(ip_or_hostname)
stream = scanner.get_point_cloud_stream()
print(stream.get_metadata())
