Skip to content

Commit

Permalink
PR #13682 from OhadMeir: Stabilize DDS tests on LibCI
Browse files Browse the repository at this point in the history
  • Loading branch information
OhadMeir authored Jan 16, 2025
2 parents 71f4938 + 87be9c2 commit 11c15bc
Show file tree
Hide file tree
Showing 4 changed files with 105 additions and 19 deletions.
84 changes: 83 additions & 1 deletion unit-tests/dds/d455.py
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,7 @@ def color_stream():
stream = dds.color_stream_server( "Color", "RGB Camera" )
stream.init_profiles( color_stream_profiles(), 10 )
stream.init_options( rgb_camera_options() )
stream.set_intrinsics( color_stream_intrinsics() )
return stream


Expand Down Expand Up @@ -398,4 +399,85 @@ def get_extrinsics():
extrinsics[("Infrared_2","Infrared_1")] = extr

return extrinsics


def color_stream_intrinsics():
intrinsics = []

intr = dds.video_intrinsics();
intr.width = 1280
intr.height = 800
intr.principal_point.x = 648.580993652344
intr.principal_point.y = 398.820983886719
intr.focal_length.x = 636.450012207031
intr.focal_length.y = 635.621948242188
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

intr = dds.video_intrinsics();
intr.width = 1280
intr.height = 720
intr.principal_point.x = 648.580993652344
intr.principal_point.y = 358.821014404297
intr.focal_length.x = 636.450012207031
intr.focal_length.y = 635.622009277344
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

intr = dds.video_intrinsics();
intr.width = 848
intr.height = 480
intr.principal_point.x = 429.684906005859
intr.principal_point.y = 239.21891784668
intr.focal_length.x = 421.648132324219
intr.focal_length.y = 421.099578857422
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

intr = dds.video_intrinsics();
intr.width = 640
intr.height = 480
intr.principal_point.x = 325.148590087891
intr.principal_point.y = 239.292602539062
intr.focal_length.x = 381.869995117188
intr.focal_length.y = 381.373199462891
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

intr = dds.video_intrinsics();
intr.width = 640
intr.height = 360
intr.principal_point.x = 324.290496826172
intr.principal_point.y = 179.410507202148
intr.focal_length.x = 318.225006103516
intr.focal_length.y = 317.811004638672
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

intr = dds.video_intrinsics();
intr.width = 480
intr.height = 270
intr.principal_point.x = 243.217880249023
intr.principal_point.y = 134.557876586914
intr.focal_length.x = 238.668746948242
intr.focal_length.y = 238.358245849609
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

intr = dds.video_intrinsics();
intr.width = 424
intr.height = 240
intr.principal_point.x = 214.84245300293
intr.principal_point.y = 119.60945892334
intr.focal_length.x = 210.824066162109
intr.focal_length.y = 210.549789428711
intr.distortion.model = dds.distortion_model.inverse_brown
intr.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
intrinsics.append( intr )

return set( intrinsics )
13 changes: 8 additions & 5 deletions unit-tests/dds/test-device-discovery.py
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ def create_server_2( root ):

# We listen directly on the device-info topic
device_info_topic = dds.message.device_info.create_topic( participant, dds.topics.device_info )
device_info = dds.topic_reader( device_info_topic )
device_info_reader = dds.topic_reader( device_info_topic )
broadcast_received = threading.Event()
broadcast_devices = []
def on_device_info_available( reader ):
Expand All @@ -81,8 +81,8 @@ def on_device_info_available( reader ):
global broadcast_devices
broadcast_devices.append( j )
broadcast_received.set()
device_info.on_data_available( on_device_info_available )
device_info.run( dds.topic_reader.qos() )
device_info_reader.on_data_available( on_device_info_available )
device_info_reader.run( dds.topic_reader.qos() )

def detect_broadcast():
global broadcast_received, broadcast_devices
Expand Down Expand Up @@ -203,6 +203,9 @@ def __exit__( self, type, value, traceback ):
reader_2 = dds.topic_reader( device_info_topic )
reader_2.run( dds.topic_reader.qos() )
test.check_equal( len(broadcast_devices), 2 )
# Add short sleep to avoid a possible deadlock. Devices broadcast is handled by `on_device_info_available` callback,
# we may still be checking for messages (in eProcima reader thread) when trying to delete reader_2.
sleep( 0.1 )
del reader_2

#############################################################################################
Expand Down Expand Up @@ -300,7 +303,7 @@ def __exit__( self, type, value, traceback ):


del watcher
device_info.stop()
del device_info
device_info_reader.stop()
del device_info_reader
del participant
test.print_results_and_exit()
21 changes: 11 additions & 10 deletions unit-tests/dds/test-librs-intrinsics.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

import pyrealdds as dds
import d435i
import d455

dds.debug( log.is_debug_on(), log.nested )

Expand All @@ -34,22 +35,22 @@ def broadcast_device( server, device_info ):

def build_scaled_intrinsics_server():
color = dds.color_stream_server( 'Color', 'RGB Camera' )
color.init_profiles( d435i.color_stream_profiles(), 8 )
color.init_profiles( d455.color_stream_profiles(), 5 )
color.init_options( [] )

# Add only a single set of intrinsics for 1920x1080, from which we expect to scale to all resolutions:
i = dds.video_intrinsics();
i.width = 1920
i.height = 1080
i.principal_point.x = 970.4506225585938
i.principal_point.y = 542.8473510742188
i.focal_length.x = 1362.133056640625
i.focal_length.y = 1362.629638671875
i.width = 1280
i.height = 720
i.principal_point.x = 648.580993652344
i.principal_point.y = 358.821014404297
i.focal_length.x = 636.450012207031
i.focal_length.y = 635.622009277344
i.distortion.model = dds.distortion_model.inverse_brown
i.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0]
i.distortion.coeffs = [-0.0569772012531757,0.0660239011049271,0.000211432998185046,0.00068545201793313,-0.0208512991666794]
color.set_intrinsics( set( [i] ) )

dev = dds.device_server( participant, d435i.device_info.topic_root )
dev = dds.device_server( participant, d455.device_info.topic_root )
dev.init( [color], [], {} )
return dev

Expand Down Expand Up @@ -167,7 +168,7 @@ def close_server( instance ):
dev = None

with test.closure( 'Scaled intrinsics' ):
remote.run( 'instance = broadcast_device( build_scaled_intrinsics_server(), d435i.device_info )' )
remote.run( 'instance = broadcast_device( build_scaled_intrinsics_server(), d455.device_info )' )
dev = rs.wait_for_devices( context, rs.only_sw_devices, n=1. )
sensors = dev.query_sensors()
test.check_equal( len(sensors), 1 )
Expand Down
6 changes: 3 additions & 3 deletions unit-tests/dds/test-md-syncer.py
Original file line number Diff line number Diff line change
Expand Up @@ -354,7 +354,7 @@ def md_thread():

def frame_callback( image, metadata ):
on_frame_ready( image, metadata ) # for reporting
sleep( 0.1 )
sleep( 0.2 )
log.d( f'<{image_id(image):->4} {dds.now()} [{threading.get_native_id()}]' )

syncer = new_syncer( on_frame_ready=frame_callback )
Expand All @@ -366,11 +366,11 @@ def frame_thread():
idstr = f'i{image_id(image)}'
log.d( f'{idstr:>5} {dds.now()} [{threadid}] enqueue {image}' )
syncer.enqueue_frame( i, image )
sleep( 0.05 )
sleep( 0.1 )

threadA = threading.Thread( target=frame_thread )
threadA.start()
sleep( 0.12 ) # Between 2nd and 3rd enqueue_frame
sleep( 0.22 ) # Between 2nd and 3rd enqueue_frame
md = new_metadata( 1, time_stamp( 1 ) )
syncer.enqueue_metadata( 1, md )
threadA.join()
Expand Down

0 comments on commit 11c15bc

Please sign in to comment.