def test_camera_timestamps_with_camera_delay_simple():
# Basic test with simple values: Camera update every 2 steps, delay of 1
# step.
platform = TriFingerPlatform(
visualization=False, enable_cameras=True, camera_delay_steps=1
)
action = platform.Action()
# set camera rate to 100 fps so we need less stepping in this test
platform.camera_rate_fps = 500
assert platform._compute_camera_update_step_interval() == 2
initial_stamp_s = platform.get_timestamp_ms(0) / 1000
# first step triggers camera (we get the initial observation at this point)
t = platform.append_desired_action(action)
assert t == 0
camera_obs = platform.get_camera_observation(t)
assert initial_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger1_stamp_s = platform.get_timestamp_ms(t) / 1000
# in second step observation is ready but still has stamp zero
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
# in third step new observation is triggered but due to delay we still get
# the old one
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger2_stamp_s = platform.get_timestamp_ms(t) / 1000
assert trigger2_stamp_s > trigger1_stamp_s
# in forth step the new observation is ready
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger2_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
# again trigger but we get previous observation due to delay
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger2_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger3_stamp_s = platform.get_timestamp_ms(t) / 1000
assert trigger3_stamp_s > trigger2_stamp_s
# and there should be an update again
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger3_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger3_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger3_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
def test_camera_timestamps_with_camera_delay_more_than_rate():
# In this test the delay is higher than the camera rate, so this results in
# the effective rate to be reduced.
# Use small numbers (camera update interval 2 and delay 3) to keep the test
# manageable.
platform = TriFingerPlatform(
visualization=False, enable_cameras=True, camera_delay_steps=3
)
action = platform.Action()
# set high camera rate so we need less stepping in this test
platform.camera_rate_fps = 500
assert platform._compute_camera_update_step_interval() == 2
initial_stamp_s = platform.get_timestamp_ms(0) / 1000
# first step triggers camera (we get the initial observation at this point)
t = platform.append_desired_action(action)
assert t == 0
camera_obs = platform.get_camera_observation(t)
assert initial_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger1_stamp_s = platform.get_timestamp_ms(t) / 1000
# now it takes 3 steps until we actually see the new observation
t = platform.append_desired_action(action)
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert initial_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
# Only now, one step later, the next update is triggered
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger2_stamp_s = platform.get_timestamp_ms(t) / 1000
assert trigger2_stamp_s > trigger1_stamp_s
# And again it takes 3 steps until we actually see the new observation
t = platform.append_desired_action(action)
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger2_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"