PointCloud publishing is laggy in Jetson Orin

Decwest commented 1 year ago

I'm using Realsense D455 on Jetson Orin, and now having trouble with lag in pointcloud publishing. As shown in this video, the right PointCloud is laggy. And it is also laggy even if only right side is connected.

https://user-images.githubusercontent.com/47899069/199919840-bbc4892a-2a58-4cd2-a89b-a618b18c572e.mp4

In addition, the following warnings appear.

04/11 16:32:34,835 WARNING [281472200065408] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
 04/11 16:32:34,886 WARNING [281472200065408] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
 04/11 16:32:34,936 WARNING [281472200065408] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ WARN] [1667547155.294717616]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547156.238700235]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547157.241694452]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547157.744978206]: No stream match for pointcloud chosen texture Process - Color
 04/11 16:32:38,090 ERROR [281471872881024] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[ WARN] [1667547159.474735792]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547160.748984701]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547161.778319174]: Still waiting for data on topics /imu/data_raw and /imu/mag...
[ WARN] [1667547162.077939427]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547163.405917373]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547164.129631328]: Packet containing angle overflow, first angle: 35966 second angle: 6
[ WARN] [1667547164.677548841]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1667547165.834187391]: No stream match for pointcloud chosen texture Process - Color

Hardware setup

PC
- NVIDIA Jetson AGX Orin
RealSense
- D455 x 2

Software setup

environment

using docker

base image
- nvcr.io/nvidia/l4t-base:r32.7.1 (≒Ubuntu 18.04)

realsense-ros installation

using apt repository

RUN apt-get install -y ros-$ROS_DISTRO-realsense2-camera
RUN apt-get install -y ros-$ROS_DISTRO-realsense2-description

RUN sudo sh -c "echo 'Acquire::https::librealsense.intel.com::Verify-Peer \"false\";' > /etc/apt/apt.conf.d/99verify-peer.conf"
RUN sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
RUN sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
RUN sudo apt-get install -y librealsense2-utils
RUN sudo apt-get install -y librealsense2-dev

launch

using rs_rgbd.launch according to https://answers.ros.org/question/396646/problem-using-pointcloud-data-in-nvidia-jetson-xavier-nx/

<launch>
  <arg name="robot_name"/>
  <arg name="align_depth"         default="true"/>
  <arg name="color_width"         default="424"/>
  <arg name="color_height"        default="240"/>
  <arg name="color_fps"         default="30"/>
  <arg name="depth_width"         default="424"/>
  <arg name="depth_height"        default="240"/>
  <arg name="depth_fps"         default="30"/>
  <arg name="infra_width"         default="424"/>
  <arg name="infra_height"        default="240"/>
  <arg name="infra_fps"         default="30"/>
  <arg name="enable_pointcloud"   default="true"/>
  <arg name="enable_infra1"   default="true"/>
  <arg name="enable_infra2"   default="true"/>
  <arg name="serial_no_camera1"             default="049122250162"/>            <!-- Note: Replace with actual serial number -->
  <arg name="serial_no_camera2"             default="046322250258"/>            <!-- Note: Replace with actual serial number -->
  <arg name="camera1"                       default="camera1"/>     <!-- Note: Replace with camera name -->
  <arg name="camera2"                       default="camera2"/>     <!-- Note: Replace with camera name -->
  <arg name="tf_prefix_camera1"         default="$(arg camera1)"/>
  <arg name="tf_prefix_camera2"         default="$(arg camera2)"/>
  <arg name="publish_tf"         default="true"/>

<!-- Right -->
  <group ns="$(arg camera1)">
    <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
      <arg name="align_depth" value="$(arg align_depth)"/>
      <arg name="color_width" value="$(arg color_width)"/>
      <arg name="color_height" value="$(arg color_height)"/>
      <arg name="color_fps" value="$(arg color_fps)"/>
      <arg name="depth_width" value="$(arg depth_width)"/>
      <arg name="depth_height" value="$(arg depth_height)"/>
      <arg name="depth_fps" value="$(arg depth_fps)"/>
      <arg name="infra_width" value="$(arg infra_width)"/>
      <arg name="infra_height" value="$(arg infra_height)"/>
      <arg name="infra_fps" value="$(arg infra_fps)"/>
      <arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
      <arg name="enable_infra1" value="$(arg enable_infra1)"/>
      <arg name="enable_infra2" value="$(arg enable_infra2)"/>
      <arg name="serial_no"             value="$(arg serial_no_camera1)"/>
      <arg name="tf_prefix"                 value="$(arg tf_prefix_camera1)"/>
      <arg name="publish_tf"                value="$(arg publish_tf)"/>
      <arg name="enable_sync"           value="true"/>
    </include>
  </group>

<!-- Left -->
  <group ns="$(arg camera2)">
    <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
      <arg name="align_depth" value="$(arg align_depth)"/>
      <arg name="color_width" value="$(arg color_width)"/>
      <arg name="color_height" value="$(arg color_height)"/>
      <arg name="color_fps" value="$(arg color_fps)"/>
      <arg name="depth_width" value="$(arg depth_width)"/>
      <arg name="depth_height" value="$(arg depth_height)"/>
      <arg name="depth_fps" value="$(arg depth_fps)"/>
      <arg name="infra_width" value="$(arg infra_width)"/>
      <arg name="infra_height" value="$(arg infra_height)"/>
      <arg name="infra_fps" value="$(arg infra_fps)"/>
      <arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
      <arg name="enable_infra1" value="$(arg enable_infra1)"/>
      <arg name="enable_infra2" value="$(arg enable_infra2)"/>
      <arg name="serial_no"             value="$(arg serial_no_camera2)"/>
      <arg name="tf_prefix"                 value="$(arg tf_prefix_camera2)"/>
      <arg name="publish_tf"                value="$(arg publish_tf)"/>
      <arg name="enable_sync"           value="true"/>
    </include>
  </group>

</launch>

I would appreciate it if you could tell me the cause and the solution. Thank you in advance.

MartyG-RealSense commented 1 year ago

Hi @Decwest Whilst there is a known issue with slow FPS or missing color when enabling pointclouds on Jetson boards - as described in https://github.com/IntelRealSense/realsense-ros/issues/1967 - your use of the rs_rgbd launch file makes me think that your particular issue may have a different cause, as pointclouds typically work normally on Jetson when using the ROS1 wrapper's rs_rgbd.launch.

As you installed from packages using the ROS wrapper's apt-get install procedure, packages installed with that method do not have CUDA graphics acceleration support enabled in librealsense. This may be causing the lag, as all of the pointcloud and alignment processing will be done on the Jetson's CPU instead of offloading the work onto the Nvidia graphics GPU on the board.

When CUDA is enabled in librealsense for computing devices with an Nvidia GPU, it accelerates color conversion, depth-color alignment and pointclouds.

To enable CUDA support, you should build librealsense and the ROS wrapper separately (librealsense installed first from packages or source code, and the wrapper built secondly from source code afterwards) instead of installing them together at the same time with apt-get install -y ros-$ROS_DISTRO-realsense2-camera

If librealsense is built from packages using the dedicated Jetson installation instructions at the link below then CUDA support will be included.

https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md#4-install-with-debian-packages

Decwest commented 1 year ago

Hi @MartyG-RealSense Thank you very much for your quick and valuable response. The method you taught me worked and the lag was eliminated.

This is how I installed it.

Install librealsense from apt repository

RUN sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
RUN sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
RUN sudo apt-get install -y librealsense2-utils
RUN sudo apt-get install -y librealsense2-dev

Install realsense ROS wrapper from source

cd ~/catkin_ws/src
git clone -b ros1-legacy https://github.com/IntelRealSense/realsense-ros.git
git clone https://github.com/pal-robotics/ddynamic_reconfigure.git
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release

MartyG-RealSense commented 1 year ago

It's great to hear that you were successful, @Decwest - thanks so much for the update and for sharing with the RealSense ROS community the details of the procedure that worked for you :)

IntelRealSense / realsense-ros