# License: Apache 2.0. See LICENSE file in root directory. # Copyright(c) 2021 Intel Corporation. All Rights Reserved. import pyrealsense2 as rs from rspy import log, test import time # Constants # domain = rs.timestamp_domain.hardware_clock # For either depth/color # # To be set before init() or playback() # fps_c = fps_d = 60 w = 640 h = 480 bpp = 2 # bytes # # Set by init() or playback() -- don't set these unless you know what you're doing! # gap_c = gap_d = 0 pixels = None device = None depth_sensor = None color_sensor = None depth_profile = None color_profile = None syncer = None playback_status = None def init( syncer_matcher = rs.matchers.default ): """ One of the two initialization functions: Use init() to initialize a software device that will generate frames (as opposed to playback() which will initialize a software device for reading from a rosbag and will NOT generate frames). This should be followed by start() to actually start "streaming". This sets multiple module variables that are used to generate software frames, and initializes a syncer automatically if needed. :param syncer_matcher: The matcher to use in the syncer (if None, no syncer will be used) -- the default will compare all streams according to timestamp """ global gap_c, gap_d gap_d = 1000 / fps_d gap_c = 1000 / fps_c # global pixels, w, h pixels = bytearray( b'\x00' * ( w * h * 2 )) # Dummy data # global device device = rs.software_device() if syncer_matcher is not None: device.create_matcher( syncer_matcher ) # global depth_sensor, color_sensor depth_sensor = device.add_sensor( "Depth" ) color_sensor = device.add_sensor( "Color" ) # depth_stream = rs.video_stream() depth_stream.type = rs.stream.depth depth_stream.uid = 0 depth_stream.width = 640 depth_stream.height = 480 depth_stream.bpp = 2 depth_stream.fmt = rs.format.z16 depth_stream.fps = fps_d # global depth_profile depth_profile = rs.video_stream_profile( depth_sensor.add_video_stream( depth_stream )) # color_stream = rs.video_stream() color_stream.type = rs.stream.color color_stream.uid = 1 color_stream.width = 640 color_stream.height = 480 color_stream.bpp = 2 color_stream.fmt = rs.format.rgba8 color_stream.fps = fps_c # global color_profile color_profile = rs.video_stream_profile( color_sensor.add_video_stream( color_stream )) # # We don't want to lose any frames so use a big queue size (default is 1) global syncer if syncer_matcher is not None: syncer = rs.syncer( 100 ) else: syncer = rs.frame_queue( 100 ) # global playback_status playback_status = None def playback_callback( status ): """ """ global playback_status playback_status = status log.d( "...", status ) def playback( filename, use_syncer = True ): """ One of the two initialization functions: Use playback() to initialize a software device for reading from a rosbag file. This device will NOT generate frames! Only the expect() functions will be available. If you use this function, it replaces init(). This should be followed by start() to actually start "streaming". :param filename: The path to the file to open for playback :param use_syncer: If True, a syncer will be used to attempt frame synchronization -- otherwise a regular queue will be used """ ctx = rs.context() # global device device = rs.playback( ctx.load_device( filename ) ) device.set_real_time( False ) device.set_status_changed_callback( playback_callback ) # global depth_sensor, color_sensor sensors = device.query_sensors() depth_sensor = next( s for s in sensors if s.name == "Depth" ) color_sensor = next( s for s in sensors if s.name == "Color" ) # global depth_profile, color_profile depth_profile = next( p for p in depth_sensor.profiles if p.stream_type() == rs.stream.depth ) color_profile = next( p for p in color_sensor.profiles if p.stream_type() == rs.stream.color ) # global syncer if use_syncer: syncer = rs.syncer( 100 ) # We don't want to lose any frames so uses a big queue size (default is 1) else: syncer = rs.frame_queue( 100 ) # global playback_status playback_status = rs.playback_status.unknown def start(): """ """ global depth_profile, color_profile, depth_sensor, color_sensor, syncer depth_sensor.open( depth_profile ) color_sensor.open( color_profile ) depth_sensor.start( syncer ) color_sensor.start( syncer ) def stop(): """ """ global depth_profile, color_profile, depth_sensor, color_sensor color_sensor.stop() depth_sensor.stop() color_sensor.close() depth_sensor.close() def reset(): """ """ global depth_profile, color_profile, depth_sensor, color_sensor color_sensor = None depth_sensor = None depth_profile = None color_profile = None # global device device = None # global syncer syncer = None def generate_depth_frame( frame_number, timestamp, next_expected=None ): """ """ global playback_status if playback_status is not None: raise RuntimeError( "cannot generate frames when playing back" ) # global depth_profile, domain, pixels, depth_sensor, w, bpp depth_frame = rs.software_video_frame() depth_frame.pixels = pixels depth_frame.stride = w * bpp depth_frame.bpp = bpp depth_frame.frame_number = frame_number depth_frame.timestamp = timestamp depth_frame.domain = domain depth_frame.profile = depth_profile # if next_expected is not None: actual_fps = round( 1000000 / ( next_expected - timestamp ) ) depth_sensor.set_metadata( rs.frame_metadata_value.actual_fps, actual_fps ) log.d( "-->", depth_frame, "with actual FPS", actual_fps ) else: log.d( "-->", depth_frame ) depth_sensor.on_video_frame( depth_frame ) def generate_color_frame( frame_number, timestamp, next_expected=None ): """ """ global playback_status if playback_status is not None: raise RuntimeError( "cannot generate frames when playing back" ) # global color_profile, domain, pixels, color_sensor, w, bpp color_frame = rs.software_video_frame() color_frame.pixels = pixels color_frame.stride = w * bpp color_frame.bpp = bpp color_frame.frame_number = frame_number color_frame.timestamp = timestamp color_frame.domain = domain color_frame.profile = color_profile # if next_expected is not None: actual_fps = round( 1000000 / ( next_expected - timestamp ) ) color_sensor.set_metadata( rs.frame_metadata_value.actual_fps, actual_fps ) log.d( "-->", color_frame, "with actual FPS", actual_fps ) else: log.d( "-->", color_frame ) color_sensor.on_video_frame( color_frame ) def generate_depth_and_color( frame_number, timestamp ): generate_depth_frame( frame_number, timestamp ) generate_color_frame( frame_number, timestamp ) def expect( depth_frame = None, color_frame = None, nothing_else = False ): """ Looks at the syncer queue and gets the next frame from it if available, checking its contents against the expected frame numbers. """ global syncer, playback_status f = syncer.poll_for_frame() if playback_status is not None: countdown = 50 # 5 seconds while not f and playback_status != rs.playback_status.stopped: countdown -= 1 if countdown == 0: break time.sleep( 0.1 ) f = syncer.poll_for_frame() # NOTE: f will never be None if not f: test.check( depth_frame is None, "expected a depth frame" ) test.check( color_frame is None, "expected a color frame" ) return False log.d( "Got", f ) fs = rs.composite_frame( f ) if fs: depth = fs.get_depth_frame() else: depth = rs.depth_frame( f ) test.info( "actual depth", depth ) test.check_equal( depth_frame is None, not depth ) if depth_frame is not None and depth: test.check_equal( depth.get_frame_number(), depth_frame ) if fs: color = fs.get_color_frame() elif not depth: color = rs.video_frame( f ) else: color = None test.info( "actual color", color ) test.check_equal( color_frame is None, not color ) if color_frame is not None and color: test.check_equal( color.get_frame_number(), color_frame ) if nothing_else: f = syncer.poll_for_frame() test.info( "Expected nothing else; actual", f ) test.check( not f ) return True def expect_nothing(): expect( nothing_else = True )