[BUG] {Unable to get IMU data rate > 100 Hz when rotation is enabled}

ehong-tl commented 8 months ago

Describe the bug Unable to get IMU data rate of more than 100 Hz from OAK-D-PRO-W when rotation is enabled.

Expected behavior To get IMU data rate as intended in camera.yaml config file.

Screenshots camera.yaml

/oak:
  ros__parameters:
    camera:
      i_calibration_dump: false
      i_enable_imu: true
      i_enable_ir: true
      i_external_calibration_path: ''
      i_floodlight_brightness: 0
      i_ip: ''
      i_laser_dot_brightness: 500
      i_mx_id: ''
      i_nn_type: none
      i_pipeline_dump: false
      i_pipeline_type: RGBD
      i_publish_tf_from_calibration: true
      i_tf_base_frame: oak
      i_tf_cam_pitch: '0.0'
      i_tf_cam_pos_x: '0.0'
      i_tf_cam_pos_y: '0.0'
      i_tf_cam_pos_z: '0.0'
      i_tf_cam_roll: '0.0'
      i_tf_cam_yaw: '0.0'
      i_tf_camera_model: ''
      i_tf_camera_name: oak
      i_tf_custom_urdf_location: ''
      i_tf_custom_xacro_args: ''
      i_tf_imu_from_descr: 'true'
      i_tf_parent_frame: oak-d-base-frame
      i_usb_port_id: ''
      i_usb_speed: SUPER_PLUS
    diagnostic_updater:
      period: 1.0
    imu:
      i_acc_cov: 0.0
      i_acc_freq: 250
      i_batch_report_threshold: 5
      i_enable_rotation: true
      i_get_base_device_timestamp: false
      i_gyro_cov: 0.0
      i_gyro_freq: 200
      i_mag_cov: 0.0
      i_max_batch_reports: 10
      i_message_type: IMU
      i_rot_cov: 0.0
      i_rot_freq: 200
      i_sync_method: LINEAR_INTERPOLATE_ACCEL
      i_update_ros_base_time_on_ros_msg: false
    left:
      i_add_exposure_offset: false
      i_board_socket_id: 1
      i_calibration_file: 'package://depthai_ros_driver/config/calibration/left_640x400.yaml'
      i_disable_node: false
      i_enable_feature_tracker: false
      i_enable_lazy_publisher: true
      i_exposure_offset: 0
      i_fps: 30.0
      i_fsync_continuous: false
      i_fsync_trigger: false
      i_get_base_device_timestamp: false
      i_low_bandwidth: false
      i_low_bandwidth_quality: 50
      i_max_q_size: 30
      i_publish_topic: true
      i_resolution: 400P
      i_reverse_stereo_socket_order: false
      i_sensor_img_orientation: NORMAL
      i_set_isp3a_fps: false
      i_simulate_from_topic: false
      i_simulated_topic_name: ''
      i_update_ros_base_time_on_ros_msg: false
      r_exposure: 1000
      r_iso: 800
      r_set_man_exposure: false
    nn:
      i_disable_resize: false
      i_enable_passthrough: false
      i_enable_passthrough_depth: false
      i_get_base_device_timestamp: false
      i_num_inference_threads: 2
      i_num_pool_frames: 4
      i_update_ros_base_time_on_ros_msg: false
    rgb:
      i_add_exposure_offset: false
      i_board_socket_id: 0
      i_calibration_file: 'package://depthai_ros_driver/config/calibration/rgb_1040x780.yaml'
      i_disable_node: false
      i_enable_feature_tracker: false
      i_enable_lazy_publisher: true
      i_enable_preview: false
      i_exposure_offset: 0
      i_fps: 30.0
      i_fsync_continuous: false
      i_fsync_trigger: false
      i_get_base_device_timestamp: false
      i_height: 720
      i_interleaved: false
      i_isp_den: 39
      i_isp_num: 10
      i_keep_preview_aspect_ratio: true
      i_low_bandwidth: false
      i_low_bandwidth_quality: 50
      i_max_q_size: 30
      i_output_isp: true
      i_preview_height: 300
      i_preview_size: 300
      i_preview_width: 300
      i_publish_topic: true
      i_resolution: 12MP
      i_reverse_stereo_socket_order: false
      i_sensor_img_orientation: NORMAL
      i_set_isp3a_fps: false
      i_set_isp_scale: true
      i_simulate_from_topic: false
      i_simulated_topic_name: ''
      i_update_ros_base_time_on_ros_msg: false
      i_width: 1280
      r_exposure: 20000
      r_focus: 1
      r_iso: 800
      r_set_man_exposure: false
      r_set_man_focus: false
      r_set_man_whitebalance: false
      r_whitebalance: 3300
    right:
      i_add_exposure_offset: false
      i_board_socket_id: 2
      i_calibration_file: 'package://depthai_ros_driver/config/calibration/right_640x400.yaml'
      i_disable_node: false
      i_enable_feature_tracker: false
      i_enable_lazy_publisher: true
      i_exposure_offset: 0
      i_fps: 30.0
      i_fsync_continuous: false
      i_fsync_trigger: false
      i_get_base_device_timestamp: false
      i_low_bandwidth: false
      i_low_bandwidth_quality: 50
      i_max_q_size: 30
      i_publish_topic: true
      i_resolution: 400P
      i_reverse_stereo_socket_order: false
      i_sensor_img_orientation: NORMAL
      i_set_isp3a_fps: false
      i_simulate_from_topic: false
      i_simulated_topic_name: ''
      i_update_ros_base_time_on_ros_msg: false
      r_exposure: 1000
      r_iso: 800
      r_set_man_exposure: false
    stereo:
      i_add_exposure_offset: false
      i_align_depth: true
      i_bilateral_sigma: 0
      i_board_socket_id: 0
      i_decimation_filter_decimation_factor: 2
      i_decimation_filter_decimation_mode: NON_ZERO_MEAN
      i_depth_filter_size: 7
      i_depth_preset: HIGH_ACCURACY
      i_disparity_width: DISPARITY_96
      i_enable_alpha_scaling: false
      i_enable_brightness_filter: true
      i_enable_companding: false
      i_enable_decimation_filter: false
      i_enable_disparity_shift: false
      i_enable_distortion_correction: false
      i_enable_lazy_publisher: true
      i_enable_spatial_filter: false
      i_enable_speckle_filter: true
      i_enable_temporal_filter: false
      i_enable_threshold_filter: true
      i_exposure_offset: 0
      i_extended_disp: true
      i_get_base_device_timestamp: false
      i_height: 780
      i_left_rect_add_exposure_offset: false
      i_left_rect_enable_feature_tracker: false
      i_left_rect_exposure_offset: 0
      i_left_rect_low_bandwidth: false
      i_left_rect_low_bandwidth_quality: 50
      i_left_socket_id: 1
      i_low_bandwidth: false
      i_low_bandwidth_quality: 50
      i_lr_check: true
      i_lrc_threshold: 10
      i_max_q_size: 30
      i_output_disparity: false
      i_publish_left_rect: true
      i_publish_right_rect: true
      i_publish_synced_rect_pair: true
      i_publish_topic: true
      i_rectify_edge_fill_color: 0
      i_reverse_stereo_socket_order: false
      i_right_rect_add_exposure_offset: false
      i_right_rect_enable_feature_tracker: false
      i_right_rect_exposure_offset: 0
      i_right_rect_low_bandwidth: false
      i_right_rect_low_bandwidth_quality: 50
      i_right_socket_id: 2
      i_set_input_size: false
      i_socket_name: rgb
      i_stereo_conf_threshold: 240
      i_subpixel: true
      i_threshold_filter_min_range: 300
      i_threshold_filter_max_range: 10000
      i_update_ros_base_time_on_ros_msg: false
      i_width: 1040
    use_sim_time: false

ros2 topic hz /oak/imu/data output from default ros2 launch depthai_ros_driver camera.launch.py

average rate: 90.822
    min: 0.000s max: 0.074s std dev: 0.02096s window: 91
average rate: 88.799
    min: 0.000s max: 0.097s std dev: 0.02309s window: 178
average rate: 89.512
    min: 0.000s max: 0.097s std dev: 0.02222s window: 269
average rate: 90.129
    min: 0.000s max: 0.097s std dev: 0.02169s window: 361
average rate: 90.081
    min: 0.000s max: 0.097s std dev: 0.02145s window: 451
average rate: 89.878
    min: 0.000s max: 0.097s std dev: 0.02144s window: 540
average rate: 90.169
    min: 0.000s max: 0.097s std dev: 0.02131s window: 632
average rate: 90.270
    min: 0.000s max: 0.097s std dev: 0.02124s window: 723
average rate: 90.238
    min: 0.000s max: 0.097s std dev: 0.02119s window: 813
average rate: 90.202
    min: 0.000s max: 0.097s std dev: 0.02114s window: 903
average rate: 90.349
    min: 0.000s max: 0.097s std dev: 0.02107s window: 995
average rate: 90.232
    min: 0.000s max: 0.097s std dev: 0.02104s window: 1084
average rate: 90.136
    min: 0.000s max: 0.097s std dev: 0.02106s window: 1173
average rate: 90.048
    min: 0.000s max: 0.097s std dev: 0.02104s window: 1262
average rate: 89.978
    min: 0.000s max: 0.097s std dev: 0.02106s window: 1351
average rate: 90.039
    min: 0.000s max: 0.097s std dev: 0.02102s window: 1442
average rate: 90.042
    min: 0.000s max: 0.097s std dev: 0.02095s window: 1535
average rate: 89.960
    min: 0.000s max: 0.097s std dev: 0.02093s window: 1624
average rate: 89.900
    min: 0.000s max: 0.097s std dev: 0.02093s window: 1713
average rate: 89.850
    min: 0.000s max: 0.097s std dev: 0.02091s window: 1802
average rate: 89.804
    min: 0.000s max: 0.097s std dev: 0.02090s window: 1891
average rate: 89.993
    min: 0.000s max: 0.097s std dev: 0.02086s window: 1985
average rate: 89.988
    min: 0.000s max: 0.097s std dev: 0.02085s window: 2075
average rate: 89.945
    min: 0.000s max: 0.097s std dev: 0.02084s window: 2164
average rate: 90.024
    min: 0.000s max: 0.097s std dev: 0.02082s window: 2256
average rate: 90.174
    min: 0.000s max: 0.097s std dev: 0.02080s window: 2350
average rate: 90.200
    min: 0.000s max: 0.097s std dev: 0.02078s window: 2441
average rate: 90.190
    min: 0.000s max: 0.097s std dev: 0.02078s window: 2531
average rate: 90.248
    min: 0.000s max: 0.097s std dev: 0.02075s window: 2626
average rate: 90.237
    min: 0.000s max: 0.097s std dev: 0.02074s window: 2716
average rate: 90.131
    min: 0.000s max: 0.097s std dev: 0.02075s window: 2803
average rate: 90.125
    min: 0.000s max: 0.097s std dev: 0.02075s window: 2893
average rate: 90.177
    min: 0.000s max: 0.097s std dev: 0.02073s window: 2985
average rate: 90.135
    min: 0.000s max: 0.097s std dev: 0.02073s window: 3074
average rate: 90.159
    min: 0.000s max: 0.097s std dev: 0.02073s window: 3165
average rate: 90.154
    min: 0.000s max: 0.097s std dev: 0.02073s window: 3255
average rate: 90.174
    min: 0.000s max: 0.097s std dev: 0.02073s window: 3346
average rate: 90.169
    min: 0.000s max: 0.097s std dev: 0.02073s window: 3436
average rate: 90.137
    min: 0.000s max: 0.097s std dev: 0.02074s window: 3525
average rate: 90.104
    min: 0.000s max: 0.097s std dev: 0.02074s window: 3614
average rate: 90.197
    min: 0.000s max: 0.097s std dev: 0.02071s window: 3711
average rate: 90.145
    min: 0.000s max: 0.097s std dev: 0.02071s window: 3799
average rate: 90.137
    min: 0.000s max: 0.097s std dev: 0.02071s window: 3889
average rate: 90.131
    min: 0.000s max: 0.097s std dev: 0.02071s window: 3979
average rate: 90.171
    min: 0.000s max: 0.097s std dev: 0.02070s window: 4071
average rate: 90.145
    min: 0.000s max: 0.097s std dev: 0.02070s window: 4160
average rate: 90.162
    min: 0.000s max: 0.097s std dev: 0.02069s window: 4251
average rate: 90.178
    min: 0.000s max: 0.097s std dev: 0.02067s window: 4342
average rate: 90.235
    min: 0.000s max: 0.097s std dev: 0.02066s window: 4435
average rate: 90.145
    min: 0.000s max: 0.097s std dev: 0.02066s window: 4521
average rate: 90.159
    min: 0.000s max: 0.097s std dev: 0.02065s window: 4612
average rate: 90.193
    min: 0.000s max: 0.097s std dev: 0.02063s window: 4704
average rate: 90.189
    min: 0.000s max: 0.097s std dev: 0.02062s window: 4794
average rate: 90.092
    min: 0.000s max: 0.097s std dev: 0.02065s window: 4879

Attach system log

System log

{
"architecture": "64bit ELF",
"machine": "aarch64",
"platform": "Linux-5.10.120-tegra-aarch64-with-glibc2.29",
"processor": "aarch64",
"python_build": "default May 26 2023 14:05:08",
"python_compiler": "GCC 9.4.0",
"python_implementation": "CPython",
"python_version": "3.8.10",
"release": "5.10.120-tegra",
"system": "Linux",
"version": "#1 SMP PREEMPT Tue Aug 1 12:32:50 PDT 2023",
"win32_ver": "",
"packages": [
    "blobconverter==1.4.2",
    "boto3==1.28.62",
    "botocore==1.31.62",
    "certifi==2023.7.22",
    "charset-normalizer==2.0.12",
    "depthai==2.21.2.0",
    "depthai-sdk==1.9.4",
    "distinctipy==1.2.3",
    "ffmpy3==0.2.4",
    "idna==3.4",
    "jmespath==1.0.1",
    "lz4==4.3.2",
    "marshmallow==3.17.0",
    "mcap==1.1.1",
    "mcap-ros1-support==0.0.8",
    "numpy==1.24.4",
    "opencv-contrib-python==4.5.5.62",
    "packaging==23.2",
    "pip==23.3",
    "pkg_resources==0.0.0",
    "python-dateutil==2.8.2",
    "pytube==15.0.0",
    "PyTurboJPEG==1.6.4",
    "pyusb==1.2.1",
    "PyYAML==6.0.1",
    "Qt.py==1.3.8",
    "requests==2.26.0",
    "rosbags==0.9.11",
    "ruamel.yaml==0.17.35",
    "ruamel.yaml.clib==0.2.8",
    "s3transfer==0.7.0",
    "scipy==1.10.1",
    "sentry-sdk==1.5.1",
    "setuptools==44.0.0",
    "six==1.16.0",
    "types-pyside2==5.15.2.1.6",
    "urllib3==1.26.17",
    "xmltodict==0.13.0",
    "zstandard==0.21.0"
],
"usb": [
    {
        "port": 1,
        "vendor_id": "0x0bda",
        "product_id": "0x0489",
        "speed": "SuperPlus"
    },
    {
        "port": 0,
        "vendor_id": "0x1d6b",
        "product_id": "0x0003",
        "speed": "SuperPlus"
    },
    {
        "port": 3,
        "vendor_id": "0x13d3",
        "product_id": "0x3549",
        "speed": "Full"
    },
    {
        "port": 4,
        "vendor_id": "0x0461",
        "product_id": "0x0010",
        "speed": "Low"
    },
    {
        "port": 3,
        "vendor_id": "0x03e7",
        "product_id": "0x2485",
        "speed": "High"
    },
    {
        "port": 2,
        "vendor_id": "0x0461",
        "product_id": "0x4e2a",
        "speed": "Low"
    },
    {
        "port": 2,
        "vendor_id": "0x0bda",
        "product_id": "0x5489",
        "speed": "High"
    },
    {
        "port": 0,
        "vendor_id": "0x1d6b",
        "product_id": "0x0002",
        "speed": "High"
    }
],
"uname": [
    "Linux orinnano1-desktop 5.10.120-tegra #1 SMP PREEMPT Tue Aug 1 12:32:50 PDT 2023 aarch64 aarch64"
]
}

Which OS/OS version are you using? Ubuntu 20.04 (Jetpack 5.1.2)
Which ROS version are you using? ROS2
Which ROS distribution are you using ? ROS2 Humble
Is depthai-ros built from source or installed from apt? From source
Is depthai/depthai-core library installed from rosdep or manually? For rosdep install, check if ros-<rosdistro>-depthai package is installed, manual install can be checked with ldconfig -p | grep depthai Installed manually

Additional context Any reason not to output rotation vector when in COPY sync mode? https://github.com/luxonis/depthai-ros/blob/4795034f539dbbef7e95168a8579b170fca1538c/depthai_bridge/src/ImuConverter.cpp#L90C6-L90C18

danilo-pejovic commented 8 months ago

Hey thanks for reporting the issue, does this issue appear on any other device or only on jetpack?

ehong-tl commented 8 months ago

Hey @danilo-pejovic

I've only tried it on Nvidia Jetson Orin Nano with ROS2 running in jetson docker container.

With this camera config, I am able to get IMU data rate of ~200 Hz in COPY mode and ~185 Hz in LINEAR_INTERPOLATE_ACCEL mode when rotation is disabled. I'm unable to get IMU data rate of more than 100 Hz when rotation is enabled regardless of the settings.

Haven't try it on PC yet, I shall find one day to try it.

ehong-tl commented 8 months ago

Hey @danilo-pejovic

I've tested on my laptop running on Ubuntu 22.04 ROS2 Humble and got the same result, IMU rate will not get over 100 Hz when rotation is enabled.

Serafadam commented 7 months ago

Hi @ehong-tl, could you test this branch and see if that improves the readings?

ehong-tl commented 7 months ago

Hi @ehong-tl, could you test this branch and see if that improves the readings?

Hi @Serafadam

I'm able to get more than 100 Hz with this branch, thanks for the fix.

luxonis / depthai-ros

[BUG] {Unable to get IMU data rate > 100 Hz when rotation is enabled} #446