T265 Camera: Frame didn't arrive within 5000
Hello,
I am currently trying to use the T265 with Python on a RaspberryPi4. I noticed that I sometimes get the Error Message: Frame didn't arrive within 5000, which causes the camera to disconnect. This behaviour occurs in two situations:
1) when the camera lenses are covered
2) randomly after some time (sometimes a few minutes, sometimes an hour, sometimes not at all)
The first case is something that I can avoid, but if it disconnects out of nowhere, I have no idea how to prevent losing connection. I already implemented automatic reconnection in my code, but in the five seconds that go by between the last dataset and the moment where I notice the lost connection, I have no idea how my location changed. Also I lose the feature of the camera that it recognises past waypoints to achieve loop closure after reconnecting.
I tried to cover the lenses while using the realsense-viewer and I noticed that it also stops recording position-data, but when I uncover the lenses the camera finds its way back to the real position, so I believe it isn't a hardware- but a software problem.
My question is if anyone has a idea on how I can achieve the same thing in my python-code and how the error could be prevented in the first place (especially the random disconnects)
Best regards,
Martin
Python Code:
import pyrealsense2 as rs
import multiprocessing as mp
import numpy as np
import ctypes as c
import math as m
import time
import cv2
#---------------------------------------------------------------------------------------------------------------------------------------------------------------
shared_pos = mp.Array(c.c_double, 7)
startTimer = time.time()
#---------------------------------------------------------------------------------------------------------------------------------------------------------------
def getPosData(poseData, offset, startTimer):
w = poseData.rotation.w
x = -poseData.rotation.z
y = poseData.rotation.x
z = -poseData.rotation.y
pitch = -m.asin(2.0 * (x*z - w*y)) * (180.0 / m.pi)
roll = m.atan2(2.0 * (w*x + y*z), w*w - x*x - y*y + z*z) * (180.0 / m.pi)
yaw = m.atan2(2.0 * (w*z + x*y), w*w + x*x - y*y - z*z) * (180.0 / m.pi) + offset[3]
x = -poseData.translation.z + offset[0]
y = -poseData.translation.x + offset[1]
z = poseData.translation.y + offset[2]
timestamp = time.time() - startTimer
returnx, y, z, pitch, roll, yaw, timestamp
def posStream(shared_pos, startTimer, lock):
t265 = rs.pipeline()
t265_config = rs.config()
t265_config.enable_device('201222111808')
t265_config.enable_stream(rs.stream.pose)
t265.start(t265_config)
offset = [0, 0, 0, 0] #x, y, z, yaw
whileTrue:
try:
frames = t265.wait_for_frames()
pose = frames.get_pose_frame()
ifpose:
lock.acquire()
shared_pos[:] = getPosData(pose.get_pose_data(), offset, startTimer)
lock.release()
exceptExceptionasex:
print("Error: {0}, Timestamp: {1:.3f}".format(ex, (time.time() - startTimer)))
lock.acquire()
pos = shared_pos[:]
lock.release()
print("Position: x={0:.3f}, y={1:.3f}, z={2:.3f}, pitch={3:.3f}, roll={4:.3f}, yaw={5:.3f}, time={6:.3f}".format(pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]))
offset[0:2] = pos[0:2]
offset[3] = pos[5]
t265.stop()
t265.start(t265_config)
#---------------------------------------------------------------------------------------------------------------------------------------------------------------
if __name__ == '__main__':
lock = mp.Lock()
processA = mp.Process(target=posStream, args=(shared_pos, startTimer, lock))
processA.daemon = True
processA.start()
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
try:
whileTrue:
lock.acquire()
pos = shared_pos[:]
lock.release()
window = np.ones((40, 840))
cv2.putText(window, str("x={0:.3f}, y={1:.3f}, z={2:.3f}, pitch={3:.3f}, roll={4:.3f}, yaw={5:.3f}, time={6:.3f}".format(pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6])), (20, 20), cv2.FONT_HERSHEY_COMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
cv2.imshow('RealSense', window)
time.sleep(0.1)
key = cv2.waitKey(1)
ifkey == 27:
break
exceptExceptionasex:
print(ex)
finally:
processA.kill()
print("Process Stopped")
-
Hi Martin,
The 'frames did not arrive' error suggests that the camera is experiencing problems with USB controller communication.
To prevent frame drop or overload of data through the USB, it has to be connected to USB 3.1 Gen 1 specification. Try connect the camera with a powerful USB 3.0 hub.
Regards,
Yu Chern
Intel Customer Support
Please sign in to leave a comment.
Comments
1 comment