search

IntelRealSense / realsense-ros

ROS Wrapper for Intel(R) RealSense(TM) Cameras
http://wiki.ros.org/RealSense
Apache License 2.0
2.6k stars 1.76k forks source link

No IMU data from #3247

Closed hellovuong closed 1 week ago

hellovuong commented 2 weeks ago
Required Info
Camera Model D455f
Firmware Version 5.15.0.2
Operating System & Version Ubuntu 20.04
Kernel Version (Linux Only) 5.10.104-tegra
Platform NVIDIA Jetson
Librealsense SDK Version 2.54.1
Language C++
Segment Robot
ROS Distro Noetic
RealSense ROS Wrapper Version ROS1-legacy (branch newest commit) run in Docker container

Issue Description

I installed SDK in Dockerfile as follows:

apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u && \
    apt-get update && \
    apt-get install librealsense2=2.54.1-0~realsense.1425 -y && \
    apt-get install -y librealsense2-gl=2.54.1-0~realsense.1425 librealsense2-dev=2.54.1-0~realsense.1425 librealsense2-utils=2.54.1-0~realsense.1425

The ROS1 wrapper is from ROS1-legacy with config as follows:

    <include file="$(find realsense2_camera)/launch/rs_aligned_depth.launch" >
        <arg name="enable_fisheye"            value="false"/>

        <arg name="depth_width"               value="848"/>
        <arg name="depth_height"              value="480"/>

        <arg name="enable_infra1"             value="false"/>
        <arg name="enable_infra2"             value="false"/>

        <arg name="color_width"               value="848"/>
        <arg name="color_height"              value="480"/>

        <arg name="accel_fps"                 value="200"/>

        <arg name="enable_pointcloud"         value="true"/>
    </include>

I checked the compatibility of SDK and firmware in https://dev.intelrealsense.com/docs/firmware-releases-d400 and it should be correct. However, I can not receive IMU data. Can you help me to debug where is the problem? Here is the output of realsense launch:

[ INFO] [1730983723.593692733]: Device with serial number 309622300196 was found.

[ INFO] [1730983723.593765600]: Device with physical ID /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.0/video4linux/video1 was found.
[ INFO] [1730983723.593791201]: Device with name Intel RealSense D455F was found.
[ INFO] [1730983723.594373719]: Device with port number 2-2 was found.
[ INFO] [1730983723.594432506]: Device USB type: 3.2
[ INFO] [1730983723.600057936]: getParameters...
[ INFO] [1730983723.753303555]: setupDevice...
[ INFO] [1730983723.753416648]: JSON file is not provided
[ INFO] [1730983723.753492682]: ROS Node Namespace: camera
[ INFO] [1730983723.753525228]: Device Name: Intel RealSense D455F
[ INFO] [1730983723.753545132]: Device Serial No: 309622300196
[ INFO] [1730983723.753564557]: Device physical port: /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.0/video4linux/video1
[ INFO] [1730983723.753594286]: Device FW version: 5.15.0.2
[ INFO] [1730983723.753617039]: Device Product ID: 0x0B5C
[ INFO] [1730983723.753638576]: Enable PointCloud: On
[ INFO] [1730983723.753655473]: Align Depth: On
[ INFO] [1730983723.753671025]: Sync Mode: On
[ INFO] [1730983723.753706675]: Device Sensors:
[ INFO] [1730983723.764566225]: Stereo Module was found.
[ INFO] [1730983723.775845151]: RGB Camera was found.
[ INFO] [1730983723.776168779]: Motion Module was found.
[ INFO] [1730983723.776661694]: Add Filter: pointcloud
[ INFO] [1730983723.777404698]: num_filters: 2
[ INFO] [1730983723.777456668]: Setting Dynamic reconfig parameters.
[ WARN] [1730983724.351202353]: Param '/camera/rgb_camera/power_line_frequency' has value 3 that is not in the enum { {50Hz: 1} {60Hz: 2} {Disabled: 0} }. Removing this parameter from dynamic reconfigure options.
[ INFO] [1730983724.377537693]: Done Setting Dynamic reconfig parameters.
[ INFO] [1730983724.378274777]: depth stream is enabled - width: 848, height: 480, fps: 30, Format: Z16
[ INFO] [1730983724.379030486]: color stream is enabled - width: 848, height: 480, fps: 30, Format: RGB8
[ INFO] [1730983724.380758712]: gyro stream is enabled - fps: 400
[ INFO] [1730983724.380833402]: accel stream is enabled - fps: 200
[ INFO] [1730983724.380864572]: setupPublishers...
[ INFO] [1730983724.383249719]: Expected frequency for depth = 30.00000
[ INFO] [1730983724.388634436]: Expected frequency for color = 30.00000
[ INFO] [1730983724.392869221]: Expected frequency for aligned_depth_to_color = 30.00000
[ INFO] [1730983724.400313249]: setupStreams...
 07/11 12:48:44,406 WARNING [281472141349232] (ds-calib-parsers.cpp:41) IMU Calibration is not available, default intrinsic and extrinsic will be used.
 07/11 12:48:44,410 WARNING [281472116171120] (backend-hid.h:68) write_fs_attribute Could not change accel_3d-dev15 to accel_3d-dev5 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/trigger/current_trigger
 07/11 12:48:44,414 WARNING [281472023916912] (backend-hid.h:68) write_fs_attribute Could not change gyro_3d-dev16 to gyro_3d-dev6 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/trigger/current_trigger
 07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
 07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
 07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
 07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
[ INFO] [1730983724.465908134]: SELECTED BASE:Depth, 0
[ INFO] [1730983724.492234418]: RealSense Node Is Up!
[ INFO] [1730983724.666834067]: publishing unordered pointcloud.
[ WARN] [1730983724.799555608]: No stream match for pointcloud chosen texture Process - Color
[ WARN] [1730983726.321769809]: No stream match for pointcloud chosen texture Process - Color
 07/11 12:48:49,422 WARNING [281472015524208] (backend-hid.cpp:633) iio_hid_sensor: Frames didn't arrived within the predefined interval
 07/11 12:48:49,422 WARNING [281472007131504] (backend-hid.cpp:633) iio_hid_sensor: Frames didn't arrived within the predefined interval

Out of those log I think this is interesting:

 07/11 12:48:44,410 WARNING [281472116171120] (backend-hid.h:68) write_fs_attribute Could not change accel_3d-dev15 to accel_3d-dev5 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/trigger/current_trigger
 07/11 12:48:44,414 WARNING [281472023916912] (backend-hid.h:68) write_fs_attribute Could not change gyro_3d-dev16 to gyro_3d-dev6 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/trigger/current_trigger
 07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
 07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
 07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
 07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
hellovuong commented 2 weeks ago

Additionally, If I installed the SDK from source with parameters -DFORCE_RSUSB_BACKEND=true the error:

 07/11 12:48:44,410 WARNING [281472116171120] (backend-hid.h:68) write_fs_attribute Could not change accel_3d-dev15 to accel_3d-dev5 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/trigger/current_trigger
 07/11 12:48:44,414 WARNING [281472023916912] (backend-hid.h:68) write_fs_attribute Could not change gyro_3d-dev16 to gyro_3d-dev6 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/trigger/current_trigger
 07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
 07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
 07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
 07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable

NOT appear any more but the frequency of color image is very low like only ~3Hz

MartyG-RealSense commented 2 weeks ago

Hi @hellovuong The message No stream match for pointcloud chosen texture Process - Color indicates that color frames are missing because they are being dropped.

The IMU topics are disabled by default in the ROS wrapper. enable_accel and enable_gyro can be set to true in the launch file if you have not done so already, or you can enable them in the roslaunch by adding to the launch instruction the following parameters:

enable_accel:=true enable_gyro:=true unite_imu_method:=linear_interpolation

The unite_imu_method combines the two separate /accel and /gyro topics into a single combined /imu topic.

hellovuong commented 2 weeks ago

Hi @MartyG-RealSense, Sorry I copied the wrong launch file. In the correct launch file, I did enable Gyro and Accel and unite method but the camera did not publish any data. I am pretty sure this warning is relevant but could not understand why it happened:

07/11 12:48:44,410 WARNING [281472116171120] (backend-hid.h:68) write_fs_attribute Could not change accel_3d-dev15 to accel_3d-dev5 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/trigger/current_trigger
07/11 12:48:44,414 WARNING [281472023916912] (backend-hid.h:68) write_fs_attribute Could not change gyro_3d-dev16 to gyro_3d-dev6 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/trigger/current_trigger
07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
07/11 12:48:44,417 WARNING [281472124563824] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200073.2.auto/iio:device15/buffer/enable
07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.h:68) write_fs_attribute Could not change 0 to 1 : path /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
07/11 12:48:44,418 WARNING [281472107778416] (backend-hid.cpp:717) HID set_power 1 failed for /sys/devices/platform/3610000.xhci/usb2/2-2/2-2:1.5/0003:8086:0B5C.0043/HID-SENSOR-200076.3.auto/iio:device16/buffer/enable
MartyG-RealSense commented 1 week ago

As you have a D455f and a Jetson, is your Jetson using JetPack 6 please?

JetPack 6 removed a function called hiddraw that RealSense cameras equipped with an IMU (known as HID devices) make use of and so these cameras will not work with JP6.

Possible fixes for this JP6 IMU issue include building librealsense from source code with RSUSB = true - as you found earlier at https://github.com/IntelRealSense/realsense-ros/issues/3247#issuecomment-2462175011 - or downgrading the JetPack version to 5.1.3.

Is the 3 Hz frequency that you experienced with the color stream improved if you add the parameter auto_exposure_priority:=false to your roslaunch instruction?

hellovuong commented 1 week ago
MartyG-RealSense commented 1 week ago

You are very welcome. I look forward to your next update. Good luck!

hellovuong commented 1 week ago

Unfortunately, the parameter auto_exposure_priority:=false doesn't help to improve the fps of the color image.

MartyG-RealSense commented 1 week ago

How does color perform if you use the roslaunch instruction below?

roslaunch realsense2_camera rs_camera.launch align_depth:=true filters:=pointcloud enable_accel:=true enable_gyro:=true unite_imu_method:=linear_interpolation color_width:=848 color_height:=480 color_fps:=30

hellovuong commented 1 week ago

I ran a couple of tests throughout today: with SDK build from source with RSUSB = true:

Do you think it can related to the USB port, cause seems like it can deliver up to a certain amount of data?

hellovuong commented 1 week ago

More detail output when I run this configuration: roslaunch realsense2_camera rs_aligned_depth.launch enable_infra1:=false enable_infra2:=false enable_fisheye:=false enable_pointcloud:=true accel_fps:=200 color_width:=848 depth_width:=848 enable_gyro:=true enable_accel:=true color image had published for some seconds until the console log printed out: 11/11 14:55:18,669 ERROR [281472049062256] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132 then no more color image :(

MartyG-RealSense commented 1 week ago

I recommend not using rs_aligned_depth.launch due to the very old age of that file (over 5 years). rs_camera.launch would be preferable. However, when rs_camera.launch has problems with publishing a pointcloud, a method that often works is to use the rs_rgbd.launch file instead.

First you will need to install support for an RGBD launch on Noetic using the command below:

sudo apt-get install ros-noetic-rgbd-launch

Then launch rs_rgbd.launch with the following instruction:

roslaunch realsense2_camera rs_rgbd.launch enable_pointcloud:=true enable_accel:=true enable_gyro:=true color_width:=848 color_height:=480 color_fps:=30

You do not need to enable alignment with this launch file as it is already enabled by default. This launch file also does not have the unite_imu_method parameter and will publish the IMU data to separate /accel and /gyro topics instead of combining them into a single /imu topic.

With the rs_rgbd launch file, the pointcloud is published to depth_image_proc instead of /camera/depth/color/points

hellovuong commented 1 week ago

Hi, Thank you for your suggestion. I figured out that this problem can be related to our kernel, I will close the issue and investigate internally. Thank you again for your active support!

MartyG-RealSense commented 1 week ago

You are very welcome. I'm glad to help. Thanks very much for the update!