Modified Realsense ROS driver runtime error and timestamps issue

Pallav1299 commented 2 years ago

This is related to issue #25. The modified_realsense_ros and librealsense2(v2.30.0) compile without error but, during runtime I get the following error:

Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

SUMMARY
========

PARAMETERS
 * /camera/aligned_depth_to_color/image_raw/disable_pub_plugins: ['image_transport...
 * /camera/aligned_depth_to_infra1/image_raw/disable_pub_plugins: ['image_transport...
 * /camera/color/image_raw/disable_pub_plugins: ['image_transport...
 * /camera/depth/image_rect_raw/disable_pub_plugins: ['image_transport...
 * /camera/infra1/image_rect_raw/disable_pub_plugins: ['image_transport...
 * /camera/infra2/image_rect_raw/disable_pub_plugins: ['image_transport...
 * /camera/realsense2_camera/accel_fps: 250
 * /camera/realsense2_camera/accel_frame_id: camera_accel_frame
 * /camera/realsense2_camera/accel_optical_frame_id: camera_accel_opti...
 * /camera/realsense2_camera/align_depth: False
 * /camera/realsense2_camera/aligned_depth_to_color_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/allow_no_texture_points: False
 * /camera/realsense2_camera/base_frame_id: camera_link
 * /camera/realsense2_camera/calib_odom_file: 
 * /camera/realsense2_camera/clip_distance: -2.0
 * /camera/realsense2_camera/color_fps: 30
 * /camera/realsense2_camera/color_frame_id: camera_color_frame
 * /camera/realsense2_camera/color_height: 720
 * /camera/realsense2_camera/color_optical_frame_id: camera_color_opti...
 * /camera/realsense2_camera/color_width: 1280
 * /camera/realsense2_camera/depth_fps: 60
 * /camera/realsense2_camera/depth_frame_id: camera_depth_frame
 * /camera/realsense2_camera/depth_height: 480
 * /camera/realsense2_camera/depth_optical_frame_id: camera_depth_opti...
 * /camera/realsense2_camera/depth_width: 640
 * /camera/realsense2_camera/device_type: 
 * /camera/realsense2_camera/emitter_on_off: True
 * /camera/realsense2_camera/enable_accel: True
 * /camera/realsense2_camera/enable_color: False
 * /camera/realsense2_camera/enable_depth: True
 * /camera/realsense2_camera/enable_emitter: True
 * /camera/realsense2_camera/enable_fisheye1: True
 * /camera/realsense2_camera/enable_fisheye2: True
 * /camera/realsense2_camera/enable_fisheye: False
 * /camera/realsense2_camera/enable_gyro: True
 * /camera/realsense2_camera/enable_infra1: True
 * /camera/realsense2_camera/enable_infra2: True
 * /camera/realsense2_camera/enable_pointcloud: False
 * /camera/realsense2_camera/enable_sync: True
 * /camera/realsense2_camera/filters: pointcloud
 * /camera/realsense2_camera/fisheye1_frame_id: camera_fisheye1_f...
 * /camera/realsense2_camera/fisheye1_optical_frame_id: camera_fisheye1_o...
 * /camera/realsense2_camera/fisheye2_frame_id: camera_fisheye2_f...
 * /camera/realsense2_camera/fisheye2_optical_frame_id: camera_fisheye2_o...
 * /camera/realsense2_camera/fisheye_fps: 30
 * /camera/realsense2_camera/fisheye_frame_id: camera_fisheye_frame
 * /camera/realsense2_camera/fisheye_height: 480
 * /camera/realsense2_camera/fisheye_optical_frame_id: camera_fisheye_op...
 * /camera/realsense2_camera/fisheye_width: 640
 * /camera/realsense2_camera/gyro_fps: 400
 * /camera/realsense2_camera/gyro_frame_id: camera_gyro_frame
 * /camera/realsense2_camera/gyro_optical_frame_id: camera_gyro_optic...
 * /camera/realsense2_camera/imu_optical_frame_id: camera_imu_optica...
 * /camera/realsense2_camera/infra1_frame_id: camera_infra1_frame
 * /camera/realsense2_camera/infra1_optical_frame_id: camera_infra1_opt...
 * /camera/realsense2_camera/infra2_frame_id: camera_infra2_frame
 * /camera/realsense2_camera/infra2_optical_frame_id: camera_infra2_opt...
 * /camera/realsense2_camera/infra_fps: 60
 * /camera/realsense2_camera/infra_height: 480
 * /camera/realsense2_camera/infra_width: 640
 * /camera/realsense2_camera/initial_reset: False
 * /camera/realsense2_camera/json_file_path: 
 * /camera/realsense2_camera/linear_accel_cov: 0.01
 * /camera/realsense2_camera/odom_frame_id: camera_odom_frame
 * /camera/realsense2_camera/pointcloud_texture_index: 0
 * /camera/realsense2_camera/pointcloud_texture_stream: RS2_STREAM_COLOR
 * /camera/realsense2_camera/pose_frame_id: camera_pose_frame
 * /camera/realsense2_camera/pose_optical_frame_id: camera_pose_optic...
 * /camera/realsense2_camera/publish_odom_tf: False
 * /camera/realsense2_camera/publish_tf: False
 * /camera/realsense2_camera/rosbag_filename: 
 * /camera/realsense2_camera/serial_no: 
 * /camera/realsense2_camera/tf_publish_rate: 0.0
 * /camera/realsense2_camera/topic_odom_in: odom_in
 * /camera/realsense2_camera/unite_imu_method: linear_interpolation
 * /camera/realsense2_camera/usb_port_id: 
 * /rosdistro: melodic
 * /rosversion: 1.14.12

NODES
  /camera/
    manager (nodelet/nodelet)
    realsense2_camera (nodelet/nodelet)

ROS_MASTER_URI=http://localhost:11311

process[camera/manager-1]: started with pid [21205]
process[camera/realsense2_camera-2]: started with pid [21206]
[ INFO] [1633606051.044659138]: Initializing nodelet with 12 worker threads.
[ INFO] [1633606051.128791832]: RealSense ROS v2.2.10
[ INFO] [1633606051.128819700]: Running with LibRealSense v2.30.0
[ INFO] [1633606051.137633175]:  
[ INFO] [1633606051.249291845]: Device with serial number 046222071488 was found.

[ INFO] [1633606051.249322275]: Device with physical ID /sys/devices/pci0000:00/0000:00:14.0/usb2/2-2/2-2:1.0/video4linux/video2 was found.
[ INFO] [1633606051.249332300]: Device with name Intel RealSense D435I was found.
[ INFO] [1633606051.250915286]: Device with port number 2-2 was found.
[ INFO] [1633606051.257984298]: getParameters...
[ INFO] [1633606051.283342250]: setupDevice...
[ INFO] [1633606051.283359794]: JSON file is not provided
[ INFO] [1633606051.283370751]: ROS Node Namespace: camera
[ INFO] [1633606051.283384351]: Device Name: Intel RealSense D435I
[ INFO] [1633606051.283394126]: Device Serial No: 046222071488
[ INFO] [1633606051.283405852]: Device physical port: /sys/devices/pci0000:00/0000:00:14.0/usb2/2-2/2-2:1.0/video4linux/video2
[ INFO] [1633606051.283417815]: Device FW version: 05.12.15.50
[ INFO] [1633606051.283431541]: Device Product ID: 0x0B3A
[ INFO] [1633606051.283442187]: Enable PointCloud: On
[ INFO] [1633606051.283453150]: Align Depth: Off
[ INFO] [1633606051.283462276]: Sync Mode: On
[ INFO] [1633606051.283642242]: Device Sensors: 
[ INFO] [1633606051.283751355]: Stereo Module was found.
[ INFO] [1633606051.283780833]: RGB Camera was found.
[ INFO] [1633606051.283796434]: Motion Module was found.
[ INFO] [1633606051.283829857]: (Fisheye, 1) sensor isn't supported by current device! -- Skipping...
[ INFO] [1633606051.283844005]: (Fisheye, 2) sensor isn't supported by current device! -- Skipping...
[ INFO] [1633606051.283855022]: (Pose, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1633606051.283957657]: Add Filter: pointcloud
[ INFO] [1633606051.286976079]: num_filters: 1
[ INFO] [1633606051.286994117]: Setting Dynamic reconfig parameters.
 07/10 16:57:31,401 WARNING [139799982233344] (types.cpp:48) hwmon command 0x7d failed. Error type: No data to return (-21).
terminate called after throwing an instance of 'rs2::invalid_value_error'
  what():  hwmon command 0x7d failed. Error type: No data to return (-21).

I also tried compiling against the default installation using sudo apt install ros-melodic-librealsense2. It works. But the issue here is that, sometimes the IMU timestamps are different from the image timestamps as shown below: Camera timestamp: camera_timestamp IMU timestamp: imu_timestamp

On the first run after connecting the device ,everything works fine, but the issue persists for the following runs.

bigsuperZZZX commented 2 years ago

In fact, I have no idea what this error means, but you can make some trials. First, make sure the version of realsense firmware, librealsense SDK, and the ros-wrapper are compatible. Second, I notice that your device runs well on the first run, so you can try to set "initial_reset" in "rs_camera.launch" to true to make the camera reset each time it starts. Also, are you willing to use this device for depth-based mapping? If only for SLAM, just cover the laser projector with a piece of black tap to eliminate the laser dots.

Pallav1299 commented 2 years ago

Thanks @bigsuperZZZX.

I'll verify the firmware, sdk and ros-wrapper compatibility.
I have already tried setting the "initial_reset" to true, but to no difference.

I am using Realsense D435i data as input to planner(depth data) and visual-inertial slam(image data). The modified_realsense_ros wrapper provides the ability to get images from Global shutter infra cameras without the projector dots as well as the depth data and suits my requirements. Am I doing something wrong here?

If we cover the laser projector, what will be the effect on depth data. Will it work well with ego_planner?

Just to clarify, what sensor did you use for ego_planner depth input and SLAM input? Did you use D435//D435i as the input to both of them?

bigsuperZZZX commented 2 years ago

Covering the laser projector will just make the depth quality less good, especially when facing a blank wall because D435/D435i uses image features to solve the depth. You can verify this in realsense-viewer. Also, ego-planner works totally OK in such situation.

We use D435 for both of them. By the way, the IMU data is acquired from the flight controller not D435 since IMU data from D435 contains really too much noise (over 10 times larger noise than from FCU).

You can compare the modified realsense-ros-warpper with the original version from github using some online text compaction websites to find ou the code that we have changed. Just several lines.

ZJU-FAST-Lab / ego-planner

Modified Realsense ROS driver runtime error and timestamps issue #31