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