You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
59 lines
1.7 KiB
59 lines
1.7 KiB
2 months ago
|
# License: Apache 2.0. See LICENSE file in root directory.
|
||
|
|
# Copyright(c) 2023 Intel Corporation. All Rights Reserved.
|
||
|
|
|
||
|
|
import pyrealsense2 as rs
|
||
|
|
from rspy import test
|
||
|
|
|
||
|
|
################################################################################################
|
||
|
|
test.start("software-device motion stream")
|
||
|
|
|
||
|
|
sd = rs.software_device()
|
||
|
|
sensor = sd.add_sensor("Motion")
|
||
|
|
|
||
|
|
intrinsics = rs.motion_device_intrinsic()
|
||
|
|
intrinsics.data = [[1.0] * 4] * 3
|
||
|
|
intrinsics.noise_variances = [2, 2, 2]
|
||
|
|
intrinsics.bias_variances = [3, 3, 3]
|
||
|
|
|
||
|
|
stream = rs.motion_stream()
|
||
|
|
stream.type = rs.stream.accel
|
||
|
|
stream.index = 0
|
||
|
|
stream.uid = 0
|
||
|
|
stream.fps = 200
|
||
|
|
stream.fmt = rs.format.motion_raw
|
||
|
|
stream.intrinsics = intrinsics
|
||
|
|
|
||
|
|
stream_profile = sensor.add_motion_stream(stream).as_motion_stream_profile()
|
||
|
|
|
||
|
|
sync = rs.syncer()
|
||
|
|
|
||
|
|
sensor.open(stream_profile)
|
||
|
|
sensor.start(sync)
|
||
|
|
|
||
|
|
frame = rs.software_motion_frame()
|
||
|
|
motion_frame_data = rs.vector()
|
||
|
|
motion_frame_data.x = 0.111
|
||
|
|
motion_frame_data.y = 0.222
|
||
|
|
motion_frame_data.z = 0.333
|
||
|
|
frame.data = motion_frame_data
|
||
|
|
frame.timestamp = 20000
|
||
|
|
frame.domain = rs.timestamp_domain.hardware_clock
|
||
|
|
frame.frame_number = 0
|
||
|
|
frame.profile = stream_profile
|
||
|
|
sensor.on_motion_frame(frame)
|
||
|
|
|
||
|
|
fset = sync.wait_for_frames()
|
||
|
|
motion = fset.first_or_default(rs.stream.accel)
|
||
|
|
motion_data = motion.as_motion_frame().get_motion_data()
|
||
|
|
|
||
|
|
test.check_equal(frame.data.x, motion_data.x)
|
||
|
|
test.check_equal(frame.data.y, motion_data.y)
|
||
|
|
test.check_equal(frame.data.z, motion_data.z)
|
||
|
|
test.check_equal(frame.frame_number, motion.get_frame_number())
|
||
|
|
test.check_equal(frame.domain, motion.get_frame_timestamp_domain())
|
||
|
|
test.check_equal(frame.timestamp, motion.get_timestamp())
|
||
|
|
|
||
|
|
test.finish()
|
||
|
|
################################################################################################
|
||
|
|
test.print_results_and_exit()
|