When using the D435i with the Nano there is a noticeable lag in both steering and throttle.

[TCIII]

SBC: NVIDIA Nano OS: Jetpack 4.4.1 Ubuntu 18.04 DC: v 4.1.0 Camera: Intel D435i depth camera Vehicle: Traxxas 4WD Slash with brushless ESC and motor

When using a PS4 gamepad for local control of my Donkey Car there is a noticeable lag in both the steering and throttle response of the car during "manage.py drive". I have experienced this lag in both DC v 3.1.5 and v 4.1.0.

I have investigated the lag in both the steering and the throttle response to determine whether it is the ESC or the Nano/PCA9685. I used a servo controller to test the Traxxas ESC and it responds correctly to the PWM range of 1.0 to 2.0 ms without hesitation. On the other hand I connected a PWM display to the throttle servo channel output on the PCA9685 to see what the throttle output was like during "manage.py drive". While using the PS4 gamepad I found that when I pushed the right joystick all the way up vertically, there was a definite delay (lag), sometimes short and sometimes long, before the throttle output PWM moved towards 2.0 ms. I experienced the same issue for reverse. There appears to be a definite issue with the drive loop's response to the PS4 gamepad throttle channel input and the appropriate output from the PCA9685 throttle channel output. To make sure that there was not an issue with the PS4 gamepad communicating with the Nano, I paired and connected the PS4 to an identical Traxxas 4WD Slash running a Nano, but instead of a D435i depth camera it was using a Sainsmart 8 Mp color camera. I found that there was no lag between the steering and throttle outputs of the gamepad joysticks and the steering and throttle responses of the Slash steering servo and ESC motor controller.

Based upon my findings, it appears that there is some issue with the Nano when using the D435i depth camera in place of the usual Rpi or Sainsmart color cameras. I have a spare Sainsmart 8 Mp camera that I can use in place of the D435i depth camera to see if the gamepad to robot steering/throttle control latency is caused by the D435i depth camera being in the drive loop.

By disconnecting the D435i depth camera from the Nano and attaching a Sainsmart 8 Mp color camera to the Nano and updating "myconfig.py" to reflect the camera changeout, I verified that the Nano/Sainsmart combination does not have the steering/throttle latency issue that the Nano/D435i depth camera does. Restoring the Nano/D435i depth camera configuration and "myconfig.py" brought back the gamepad steering and throttle latency as I originally experienced. Even updating/upgrading Ubuntu did not cure the latency problem.

I have been working on the Nano/D435i camera project with a DC member in Australia who is using the same configuration, but is much farther ahead with the project than me. Attempts to contact him through Discord Direct Messages have been to no avail as he has been silent for three weeks so I do not have an experienced user who has performed data collection and training to fall back on.

Comments please.

[zlite]

I've seen the same thing. I think the librealsense pipe polling module is not playing nicely with the donkey parts polling loop for some reason. On the RPi 4, Realsense consistantly times out ("Frame not received in 5000").

Have you tried switching to 10w mode on the Nano to see if that makes a difference?

[TCIII]

@zlite,

Since I am powering my Nano with an Anker 13 amp-hr battery I always run in the 10 watt four processor mode.

I hope that I hear from my FB DC friend in Australia soon as he has actually collected data and trained on the Nano using the same combination that I am using and might be able to shed some light on the latency issue. The "realsense435i.py" part was written by Ed Murphy almost two years ago and he does not appear to have a Github account.

Here is what Ed Murphy had to say on the NVIDIA Developer forum concerning his effort to write the realsense435i.py code.

[TCIII]

@zlite,

Since the T265 tracking camera works without the latency issue on a Rpi 4B using the "path_follow.py" template, I believe that the cause of the latency is that "realsense435i.py" is capturing both video and IMU data which the T265 part did do. The T265 only used IMU data. I am going to experiment by trying lower H and V widths at 30 Hz to see if reducing the window aperture will reduce the latency.

[TCIII]

@zlite,

I have tried reducing the video frame window from the H =424, V=240 used by the member in Australia to no avail. However, I did have a somewhat noticeable reduction in the latency by updating the D435i firmware from 5.12.2 to 5.12.9.

A review of Ed Murphy's comments embedded in the "realsense435.py" part indicates that he used a video frame window of H=424, V=240 at 60 Hz. The member in Australia made the "DRIVE_LOOP_HZ = 30" and "CAMERA_FRAMERATE = DRIVE_LOOP_HZ". I have tried making the "DRIVE_LOOP_HZ = 20", which is the default value, and the "CAMERA_FRAMERATE = 60" per Murphy's recommendation. This did not appear to change the initial latency period except that once I got the car responding to the PS4 gamepad throttle input I had continuous control of the throttle and steering with out any hesitation as I had previously experienced. This needs further investigation.

Additionally I have tried contacting Ed Murphy, the author of the "realsens435.py" part though his YouTube video comments found here.

[Ezward]

Hi @zlite . The YouTube comment as a good idea! So my first question is; how much latency are you seeing with 5.12.2 firmware and how much with 5.12.9 firmware? As you probably could read in the issues I opened with Intel, making the RealSense pipeline work with RBG, Depth and IMU data was tricky; I did not see any Intel examples that tried to stream all of those simulatneously. So I was in the 'get it to work' mode. So there are probably optimizations that could be done. The reading of the pipeline should be happening in a thread, so it should not block.

I put a lot of detail into the Realsense github issues I opened and those I commented on. You can find them here: https://github.com/IntelRealSense/librealsense/issues?q=commenter%3AEzward+

[Ezward]

So, the driver I wrote uses two pipelines and that may be the issue; see https://github.com/IntelRealSense/librealsense/issues/6370#issuecomment-626398001 This issue describes how to setup callbacks in python: https://github.com/IntelRealSense/librealsense/issues/5417. I can take a look at this on the weekend.

[TCIII]

@Ezward,

Thanks for chiming in, much appreciated. Actually it was me, TCIII, who wrote the comment on your YouTube video.

I also have an Intel T265 tracking camera paired with a Rpi 4B 4GB running DC v 4.1.0 but using the "path_follow.py" template instead of the "complete.py" template. This DC version does not attempt to stream the video along with the IMU data and records steering, throttle, and imu data in the python pickle format. There is no steering or throttle latency whatsoever with the Rpi 4B/T265 tracking camera configuration.

Basically the latency symptoms occurred with both DC v 3.1.5 and v 4.1.0. When I run "manage.py drive" I have instant control of the car steering, however it is not smooth and kind of moves in increments from center to right or left if I push the PS4 gamepad left joystick from center all the way to the right or left horizontally. As for the throttle, initially I have to push the PS4 gamepad right joystick all the way up vertically and wait about five seconds for the throttle to respond. After the initial delay, the throttle will respond fairly quickly to changes in the throttle joystick vertical position, but fine throttle speed resolution is tricky. I believe that I noticed a reduction in latency and and increase in smoothness in the steering and throttle responses when I updated the D435i firmware from v 5.12.2 to v 5.12.9 today, but the initial latency to the initial throttle movement never changed. I will be glad to work with you to investigate and reduce or eliminate the latency problem when using the Intel D435i with the DC OS.

[TCIII]

@Ezward,

You appear to be using OpenCV in the "realsense435.py" part and OpenCV has undergone a number of upgrades since Apr 2019 that could possibly be causing an issue with your original code?

[Ezward]

@TCIII the only thing I am doing with OpenCV is resizing and converting to grayscale. It's not impossible that OpenCV has an issue, but I'm going to focus on the RealSense firmware, since I have seen a case where a firmware update basically broke IMU streaming. I think the D435i variant is not well loved at Intel. There are literally zero examples of streaming IMU, RGB and Depth simultaneously either in C++ or Python. I had to figure that all myself. I know this code is somewhat unstable; if it fails it usually fails at initialization while waiting for frames. I don't like that API; one of the linked issues shows how to use callbacks in Python. I think that would work better than waiting for frames. So first I will look at reproducing the issue, then I will try firmware versions, then a will refactor using call backs.

[TCIII]

@Ezward,

Thanks for the insights concerning OpenCV and the dearth of Intel D435i code examples, much appreciated. I understand your frustration. The D435i update that I performed went from v 5.12.2 to v 5.12.9. However the Intel Firmware Update website indicates that there is a v 5.12.11 available. I tried a firmware update from v 5.12.9, but it will only update with the recommended v 5.12.9. Have you seen these Intelsense Python Bindings recently? Intel Streaming Depth Python code. A Github solution for call backs. A frame que example for two processes running at the same time. Useful "callback" links: Link1 Link2 Link3 Link4

I am available this weekend if you want a test subject available.

[TCIII]

@Ezward

Did you have a chance to look at your "realsens435i.py" part last weekend as you said that you might possibly do?

[Ezward]

@TCIII I'm sorry I did not get a chance. I will get to it this weekend. One thing to try is to not stream IMU data. I'm not sure if your application requires IMU data, but if it does not, definitely turn that off. Then the driver is very standard looking streaming RGB and depth together on a single pipeline. Also, you can run the driver in 'self-test' mode; it has a main so you can execute it directly; can you give that a try and see if you think that has the same issue? And thank you for all the detail on the firmware versions you are using; I have seen that performance can vary between versions, especially for the 435i.

[TCIII]

@Ezward

Thanks for the update, much appreciated.

Glad to be of help.

Presently I am just a novice Python programmer, but I will try and give your suggestion a shot. Should I "cd to projects/donkeycar/donkeycar/parts" and run "python realsense435i.py main" while using the Desktop terminal?

[Ezward]

Just python realsense435i.py, it will automatically look for a main. You probably need the donkeycar virtual environment activated.

[TCIII]

@Ezward,

Thanks for the input, much appreciated. I will give it a shot this weekend.

@zlite and I are working on getting the Slamtech RPLidar A1M8 integrated into the DC configuration using lidar.py also.

[TCIII]

@Ezward,

Per your request, I ran python realsense435i.py from the parts directory and here is a sample of what was sent to the CLI via SSH:

"(env) thomas@thomas-desktop:~/projects/donkeycar/donkeycar/parts$ python3 realsense435i.py The output images will be resized from (424, 240, 3) to (212, 120, 3). This requires opencv. Depth Scale is: 0.0010000000474974513 imu frame 1 in 0.38933682441711426 seconds: accel = (-0.009806649759411812, -9.110377311706543, -1.7750035524368286), gyro = (-0.005235987715423107, 0.0, 0.0) imu frame 2 in 0.23500871658325195 seconds: accel = (-0.029419949278235435, -9.110377311706543, -1.794616937637329), gyro = (-0.003490658476948738, 0.001745329238474369, 0.0) imu frame 3 in 0.23468327522277832 seconds: accel = (-0.029419949278235435, -9.110377311706543, -1.794616937637329), gyro = (-0.003490658476948738, -0.003490658476948738, 0.0) imu frame 4 in 0.23511433601379395 seconds:"

Now, when I ran realsense435i.py from the Desktop and set "show_opencv_window = True", I got the same imu frame data plus two windows opened. One with the RGB camera video and one with the Depth camera.

There did not appear to be any hesitancy or latency during either the CLI data output or the Desktop data output and RGB camera and Depth camera video.

Was this output what you were expecting with the "self test"?

Comments?

The DC member in Australia never mentioned any latency like ChrisA and I are seeing and I suspect that it is because he was running his D435i on a NVIDIA Xavier NX running DC 3.1.5. I do have an Xavier NX that I could try with DC 4.1.0 and the 435i camera.

[TCIII]

@Ezward

Did you have a chance to look at your "realsens435i.py" part this last weekend as you said that you might possibly do?

Per your suggestion, I turned off the D435i IMU by enabling the following: "REALSENSE_D435_IMU = False". Disabling the D435i IMU input had no affect on the latency issue. I still had incremental steering movement and it took at least 5-10 seconds for the motor to respond to the initial throttle input and then there was a 2-3 second delay to any further throttle input.

[TCIII]

@Ezward,

Will you have a chance to look at your "realsens435i.py" part this weekend as you said that you might possibly do?

[Ezward]

@TCIII , sorry for the silence. I'm working on this now.

• Bummer that turning off IMU did not have an effect; that was where I was going to look first.
• Yes, that is what the main() should do (not really a self test, but a self-hosting example where you can easily turn on and off options). It's interesting that the main() appears to works smoothly, but the code does not work smoothly when running as a DonkeyCar part. As a donkeycar part it is polled in a thread; the main thread simply returns that last values that the thread update() wrote when run_threaded() is called, but the same basic code is executing; I would expect to see stuttering frames if it was hesitating returning a frame.
• It sounds like I really need to duplicate the issue you are seeing before I go off and change anything. I don't remember the same issue with my setup; I actually got my 1/10th build to do sub-10 second laps consistently, so steering and throttle where not an issue. I'll see if I can make it happen with the same firmware you are using.
• One thing that is worth saying; you should be using the USB cable that came with the camera or a certified SuperSpeed (SS) cable. A random cable will cause lots of problems. It doesn't sound like that is the issue you are seeing, but it is worth mentioning.

[Ezward]

I've not been able to reproduce the issue with my setup; donkeycar 3.1 commit 77b1dc76, Realsense SDK v2.32.1, firmware 05.12.03.00. I don't have a PWM viewer, so I'm just looking at the response of throttle and steering by observing the car suspended in a box. I see no difference between the 'MOCK' camera (no camera) and the 'D435' camera; both respond almost instantly to game controller inputs. I'm using an XBox controller, but I would not think that matters.

My next step is to use Donkeycar 4.1 to try to duplicate. Working on that now.

[TCIII]

@Ezward,

Thank you for your update and progress, much appreciated. I am using the USB cable that came with the D435i. Are you using a Nano or an Rpi 4B? If you are using a Nano, make sure that you are running Jetpack 4.4.3 and do not upgrade to Jetpack 4.5.1. I have found that DC 4.2.0 (dev) running on v 4.5.1 is experiencing an illegal command core dump that is not recoverable and requires a complete reinstallation of Jetpack 4.5.1. Using DC v 4.1.0 will not cause an illegal command core dump error.

[TCIII]

@Ezward,

Have you had a chance to see if you can duplicate the steering and throttle latency that I am seeing using DC 4.1.0 (master) which would indicate that it is a DC software issue and not your "realsense435i.py" part? You indicated that you are running Realsense SDK v 2.32.1 and firmware 5.12.03 where as I am running SDK v 2.40 and firmware 5.12.09 and changes in the SDK and the firmware might explain why I am having a latency issue and you are not.

[TCIII]

@Ezward,

Bump?

I tried upping the drive loop frequency to 60Hz which is the same as the D435i frame rate and found a faster response in the steering to the gamepad steering joystick input, but the steering is still somewhat incremental and not smooth. The throttle still has the initial startup latency and still does not responding quickly to the gamepad throttle joystick input.

I have tried both DC v 4.1.0 (master) and v 4.2.0 (dev) and get the same latency and control response issues, using the drive_loop = 30 with both the PS4 and the XBox One gamepads. The 30 Hz drive_loop seems to improve both the steering and throttle response over the 20 Hz drive_loop. The steering responds more smoothly and there is no initial latency. However there is a noticeable startup lag with the throttle and then the throttle responds reasonably well to further right joystick vertical input.

One thing I have noticed in both v 4.1 and 4.2 is that the XBox One right joystick vertical axis, which controls the throttle has become messed up in those two versions. Presently I can only control the forward speed by pulling right joystick down vertically and if I press it up vertically there is no reverse. If I set JOYSTICK_THROTTLE_DIR = 1.0, then pulling the right joystick down vertically does result in reverse speed control, however when pressing it up vertically there is no forward speed control at all. The PS4 gamepad does not act this way.

[TCIII]

@Ezward,

Zlite recommended that I update librealsense v 2.40 to v 2.44 which I have done.

Realsense Viewer recommended updating the D435i firmware from v 5.12.09 to v 5.12.12 which I did.

All three D435i channels (Stereo Module/RGB Camera/Motion Module/) are functional in the Realsense Viewer.

However, after updating librealsense to v 2.44 I am still experiencing the same latency/hesitation issues with the steering/throttle, so still no joy! Initially it took about 10 seconds before the throttle began to respond to the gamepad throttle joystick. The steering responds right at startup, though it does not respond smoothly to the gamepad steering joystick movement.

I really would appreciate it if you could continue to work with me on getting the Nano/D435i DC configuration to work without the latency/hesitation issue.

[TCIII]

@Ezward,

I finally found a solution to the Nano/D435i DC configuration: You have to use a Xavier NX if you want to run without the startup latency and hesitation I have been experiencing with the Nano.

I started out using the same myconfig.py configuration as the Discord member in Australia, who was also using a Xavier NX/D435i DC configuration, where the Drive Loop frequency and the Camera Frame Rate were the same at 30 Hz. After starting manage.py drive I found the steering response to my PS gamepad to be smooth and without latency or hesitation. The throttle still had an initial startup latency, however it was about one quarter as long as when running on the Nano. After the initial throttle startup latency, the throttle responded without hesitation to input from my PS4 gamepad right joystick.

So I shutdown and changed the Camera Frame Rate to 60 Hz, as you have recommended, but kept the Drive Loop frequency at 30Hz, and started manage.py drive again. This time there was no throttle startup latency and the throttle again responded without hesitation to input from my PS4 gamepad right joystick.

Based on the results of this testing using both the Xavier NX and the Nano, it has become obvious that it is necessary to use a Xavier NX, when running a version of Jetpack less than 4.5.1, to run with the D435i depth camera without steering or throttle latency or hesitation.

[TCIII]

Today I ran a Nano 4GB running JP 4.5.1 and DC 4.1 with the D435i depth camera. Running JP 4.5.1 resulted in the steering and throttle performing normally without latency or hesitation. The TubWriter average time was less than 30ms using a SandDisk 64GB Ultra micro SD card which is a far cry from the 1.5 seconds when running JP 4.4.1. This empirical testing indicates that some versions of JP (4.4.1) prior to JP 4.5.1 would fall back to a SD card write speed of HS and not UHS at boot up and after a reboot. Therefore a SD card with a minimum write speed of 10Mbs/sec will more than meet the DC TubWriter speed requirements.

[Ezward]

@TCIII Thanks for posting this. That explains a lot. I had no luck reproducing the issue; my Nano is using a fast SD card (write it 40Mbs/sec) so I would not see the issue. Booting from a solid state drive then should also work; https://www.jetsonhacks.com/2021/03/10/jetson-nano-boot-from-usb/

[TCIII]

@Ezward, It doesn't matter how fast your SD card is, JP 4.4.0/1 will only write at HS (High Speed) and not UHS (Ultra High Speed) speeds. HS is limited to a maximum of 10Mbs/sec or slower which is not fast enough for the DC Drive_Loop given the amount of data being streamed from the D435i depth camera. The Drive_Loop needed a write speed around 20Mbs/sec (UHS) to not seriously affect the response of the Drive_Loop as I was observing with the steering and throttle latency/hesitation.