PyRealsense wrapper - capturing asynchronous streams (IMU, depth, color)

[chronma]

---

Required Info
Camera Model	{ L515 }
Firmware Version	( 01.05.08.01)
Operating System & Version	{Win10) / Linux (Ubuntu 20.04)
Kernel Version (Linux Only)	(?)
Platform	PC.
SDK Version	{ 2 }
Language	{python3 }
Segment	{others }

Hello,
1)
There is a slight bug with the L515 the gyro and accelorometer stream profiles titles are swapped when calling the data,
index 4 (frame[4]] yields gyro but is listed as "pyrealsense2.stream_profile: Accel(0) @ 200fps MOTION_XYZ32F"
index 3 (frame[3]] yields accel but is listed as "pyrealsense2.stream_profile: Gyro(0) @ 200fps MOTION_XYZ32F"

I was wondering if there was a way to thread the capture of separate streams using pyrealsense2 wrapper.
The concept is to capture and the gyro and accelerometer data frame by frame as well as depth and color frame by frame. Similar to what i think the SDK example RS-callback is doing in C++

My ultimate goal is to create a hand held "video camera" that can track its location using the gyro and accelerometer and allow for the capture of pointcloud data at the push of a button at various places. similar in function to Rtab mapper, ssl_slam, but with out the ROS overhead, cumbersome interfaces, and odometry / point stripping.

Here is output from a test file i was playing with. The distance frame is captured frame by frame, but the gyro isn't (30/200fps)
Device PID: 0B64 Device name: Intel RealSense L515 Serial number: f0244791 Firmware version: 01.05.08.01 USB: 3.2 camera init complete <pyrealsense2.[video_]stream_profile: Depth(0) 640x480 @ 30fps Z16> <pyrealsense2.[video_]stream_profile: Infrared(0) 640x480 @ 30fps Y8> <pyrealsense2.[video_]stream_profile: Color(0) 1280x720 @ 30fps RGB8> <pyrealsense2.stream_profile: Gyro(0) @ 200fps MOTION_XYZ32F> <pyrealsense2.stream_profile: Accel(0) @ 200fps MOTION_XYZ32F> accelerometer: x: 0.11768, y: -9.89491, z: 0.804145 gyro: x: -0.00523599, y: 0, z: 0 gyro frame #: 403 distance: 0.0 dist frame #: 24 accelerometer: x: 0.11768, y: -9.91452, z: 0.794339 gyro: x: -0.00174533, y: 0, z: 0 gyro frame #: 416 distance: 0.0 dist frame #: 25 accelerometer: x: 0.11768, y: -9.91452, z: 0.794339 gyro: x: -0.00174533, y: 0, z: 0 gyro frame #: 416 distance: 0.0 dist frame #: 26

# l515 camera test 
import pyrealsense2 as rs
import numpy as np
import time
from PIL import Image as im
import subprocess
def initialize_camera():
    ctx = rs.context()
    devices = ctx.query_devices()
    for dev in devices:
        print('  Device PID: ',  dev.get_info(rs.camera_info.product_id))
        print('  Device name: ',  dev.get_info(rs.camera_info.name))
        print('  Serial number: ',  dev.get_info(rs.camera_info.serial_number))
        print('  Firmware version: ',  dev.get_info(rs.camera_info.firmware_version))
        print('  USB: ',  dev.get_info(rs.camera_info.usb_type_descriptor))
    serial=dev.get_info(rs.camera_info.serial_number)
    # start the frames pipe
    p = rs.pipeline()
    conf = rs.config()
    #conf.enable_device(serial)   #test of enable device
    prof = p.start(conf)
    print ('camera init complete')
    time.sleep(2)
    return p, prof
print('got here')
p,q = initialize_camera()
stream_list =q.get_streams()
for i in stream_list:
    print (i)
try:
    for i in range(10):
        f = p.wait_for_frames()
        accel = (f[3].as_motion_frame().get_motion_data())
        gyro =  (f[4].as_motion_frame().get_motion_data())
        #color = f.get_color_frame()
        #infrared = f.get_infrared_frame()
        depth = f.get_depth_frame()
        dist = (depth.get_distance(240, 320))
        #col = np.asanyarray(color.get_data())
        #data=im.fromarray(col)
        #data.save('image.png')
        #IRcol = np.asanyarray(infrared.get_data())
        #IRdata=im.fromarray(IRcol)
        #IRdata.save('image1.png')
        print("accelerometer: ", accel)
        print("gyro: ", gyro)
        print('gyro frame #: ',f[4].get_frame_number())
        print('distance: ', dist)
        print('dist frame #: ',f[0].get_frame_number())
        #print('color: ', (col[400,400]))
        #print(data)
        #print('col array shape',col.shape)
        #print('infrared: ', (IRcol[240,320]))
        #time.sleep(0)
        #subprocess.check_call('cls',shell=True)
finally:
    p.stop()`

[MartyG-RealSense]

Hi @chronma

1. A RealSense team member provides advice about retrieving and printing gyro and accel values in an L515 case at Problem reading IMU data with L515 #6896 (comment) where they state that assuming the IMU stream indexes are fixed values instead of determining them dynamically can lead to the swapping of accel and gyro data that you described.

2. You could try putting the streams on two separate pipelines, putting IMU on one pipeline on its own and depth + color on the other pipeline. An example of a Python script for doing so is at D435i cannot stream imu, rgb and depth simultaneousely #5628 (comment)

[chronma]

Thanks MartyG!

I will try to massage the example into a threaded version.

[chronma]

Here's an example of threading 4 streams individually. L515 camera

(new.venv) PS C:\Users\office-win\Desktop\vscode> 
index: 99  dpth :99                              index: 99 accel: 99

index: 99 rgb: 99                                index: 99 gyro: 99
elapsed time: 3.8737740516662598

#L515 camera
import pyrealsense2 as rs
import time
import subprocess
import sys
from threading import Thread

def print_there(x, y, text):
     sys.stdout.write("\x1b7\x1b[%d;%df%s\x1b8" % (x, y, text))
     sys.stdout.flush()

def accel_fps():

  for i in range(0, 100):
    accel_frames = accel_pipe.wait_for_frames()
    if i==0:
      startframe=accel_frames.get_frame_number()
      outstring = "index: "+str(i)+" accel "+str(accel_frames.get_frame_number())+" "
    else:
      outstring = "index: "+str(i)+" accel: "+str(accel_frames.get_frame_number()-startframe)+" "
    print_there(2,50,outstring)

def gyro_fps():

  for i in range(0, 100):
    gyro_frames = gyro_pipe.wait_for_frames()
    if i==0:
      startframe=gyro_frames.get_frame_number()
      outstring = "index: "+str(i)+" gyro: "+str(gyro_frames.get_frame_number()-startframe)+" "
    else:
      outstring = "index: "+str(i)+" gyro: "+str(gyro_frames.get_frame_number()-startframe)+" "
    print_there(4,50,outstring)

def depth_fps():
  for i in range(0, 100):
    frames = depth_pipe.wait_for_frames()
    depth_frame=frames.get_depth_frame()
    if i==0:
      startframe=depth_frame.get_frame_number()
      outstring = "index: "+str(i)+"  dpth :"+str(depth_frame.get_frame_number()-startframe)+" "
    else:
      outstring = "index: "+str(i)+"  dpth :"+str(depth_frame.get_frame_number()-startframe)+" "
    print_there(2,0,outstring)
def rgb_fps():
  for i in range(0, 100):
    frames = rgb_pipe.wait_for_frames()
    color_frame=frames.get_color_frame()
    if i==0:
      startframe=color_frame.get_frame_number()
      outstring = "index: "+str(i)+" rgb: "+str(color_frame.get_frame_number()-startframe)+" "
    else:
      outstring = "index: "+str(i)+" rgb: "+str(color_frame.get_frame_number()-startframe)+" "
    print_there(4,0,outstring)

try:
  #accel pipeline    
  accel_pipe = rs.pipeline()
  accel_config = rs.config()
  accel_config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 200) # acceleration
  accel_profile = accel_pipe.start(accel_config)
  #gyro pipeline 
  gyro_pipe = rs.pipeline()
  gyro_config = rs.config()
  gyro_config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200)  # gyroscope
  gyro_profile = gyro_pipe.start(gyro_config)
  #depth pipeline
  depth_pipe = rs.pipeline()
  depth_config = rs.config()
  depth_config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)  # depth
  depth_profile = depth_pipe.start(depth_config)
  #color pipeline
  rgb_pipe = rs.pipeline()
  rgb_config = rs.config()
  rgb_config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)  # rgb
  rgb_profile = rgb_pipe.start(rgb_config)
 
  subprocess.check_call('cls',shell=True)
  t1=Thread(target=accel_fps)
  t2=Thread(target=gyro_fps)
  t3=Thread(target=depth_fps)
  t4=Thread(target=rgb_fps)
  
  starttime=time.time()
  t1.start()
  t2.start()
  t3.start()
  t4.start()
  
  t1.join()
  t2.join()
  t3.join()
  t4.join()


finally:
    print_there (5,0,str('elapsed time: '+str(time.time()-starttime)))
    accel_pipe.stop()
    gyro_pipe.stop()
    depth_pipe.stop()
    rgb_pipe.stop()

[MartyG-RealSense]

Thanks so much @chronma for sharing your threading code for the L515 with the RealSense community :)

[MartyG-RealSense]

Hi @chronma Do you require further assistance with this case, please? Thanks!

[chronma]

@MartyG-RealSense, Marty i think I'm good, thanks.

[MartyG-RealSense]

Thanks very much @chronma for the update!