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

problems in receiving IMU data #3236

Closed dyb678 closed 1 week ago

dyb678 commented 3 weeks ago

Hi ! I use the D435i and want to get the imu data. But I find I have the topic"/camera/imu" in my rostopic list. But I cann't receive the data .My launch file is this:

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>

  <arg name="fisheye_width"       default="640"/>
  <arg name="fisheye_height"      default="480"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="640"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="infra_width"        default="640"/>
  <arg name="infra_height"       default="480"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>

  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="30"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="30"/>
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="200"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>

  <arg name="enable_sync"               default="true"/>
  <arg name="align_depth"               default="true"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="unite_imu_method"          default="linear_interpolation"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="emitter_enable"        default="false"/>

<!-- rosparam set /camera/stereo_module/emitter_enabled false -->
<rosparam>
  /camera/stereo_module/emitter_enabled: 0
</rosparam>

<rosparam if="$(arg emitter_enable)">
  /camera/stereo_module/emitter_enabled: 1
</rosparam>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
    </include>
  </group>
</launch>

And its corresponding output in terminal is this：...

 logging to /home/nuc/.ros/log/7d9d19a6-9503-11ef-87a6-88d82e05ed67/roslaunch-nuc-14153.log
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.

started roslaunch server http://nuc:42881/

SUMMARY
========

PARAMETERS
 * /camera/realsense2_camera/accel_fps: 200
 * /camera/realsense2_camera/accel_frame_id: camera_accel_frame
 * /camera/realsense2_camera/accel_optical_frame_id: camera_accel_opti...
 * /camera/realsense2_camera/align_depth: True
 * /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: 480
 * /camera/realsense2_camera/color_optical_frame_id: camera_color_opti...
 * /camera/realsense2_camera/color_width: 640
 * /camera/realsense2_camera/confidence_fps: 30
 * /camera/realsense2_camera/confidence_height: 480
 * /camera/realsense2_camera/confidence_width: 640
 * /camera/realsense2_camera/depth_fps: 30
 * /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/enable_accel: True
 * /camera/realsense2_camera/enable_color: True
 * /camera/realsense2_camera/enable_confidence: True
 * /camera/realsense2_camera/enable_depth: True
 * /camera/realsense2_camera/enable_fisheye1: False
 * /camera/realsense2_camera/enable_fisheye2: False
 * /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_infra: False
 * /camera/realsense2_camera/enable_pointcloud: False
 * /camera/realsense2_camera/enable_pose: False
 * /camera/realsense2_camera/enable_sync: True
 * /camera/realsense2_camera/filters: 
 * /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: 200
 * /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: 30
 * /camera/realsense2_camera/infra_height: 480
 * /camera/realsense2_camera/infra_rgb: False
 * /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/ordered_pc: False
 * /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: True
 * /camera/realsense2_camera/publish_tf: True
 * /camera/realsense2_camera/reconnect_timeout: 6.0
 * /camera/realsense2_camera/rosbag_filename: 
 * /camera/realsense2_camera/serial_no: 
 * /camera/realsense2_camera/stereo_module/exposure/1: 7500
 * /camera/realsense2_camera/stereo_module/exposure/2: 1
 * /camera/realsense2_camera/stereo_module/gain/1: 16
 * /camera/realsense2_camera/stereo_module/gain/2: 16
 * /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: 
 * /camera/realsense2_camera/wait_for_device_timeout: -1.0
 * /camera/stereo_module/emitter_enabled: 0
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /camera/
    realsense2_camera (nodelet/nodelet)
    realsense2_camera_manager (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [14163]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 7d9d19a6-9503-11ef-87a6-88d82e05ed67
process[rosout-1]: started with pid [14174]
started core service [/rosout]
process[camera/realsense2_camera_manager-2]: started with pid [14182]
[ INFO] [1730102770.265613140]: Initializing nodelet with 8 worker threads.
process[camera/realsense2_camera-3]: started with pid [14183]
[ INFO] [1730102770.367813672]: RealSense ROS v2.3.2
[ INFO] [1730102770.367838047]: Built with LibRealSense v2.50.0
[ INFO] [1730102770.367845131]: Running with LibRealSense v2.50.0
[ INFO] [1730102770.386404756]:  
 28/10 16:06:10,443 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Success, number: 0
 28/10 16:06:10,583 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Success, number: 0
[ INFO] [1730102770.647191551]: Device with serial number 332322073932 was found.

[ INFO] [1730102770.647259383]: Device with physical ID 4-3-3 was found.
[ INFO] [1730102770.647285147]: Device with name Intel RealSense D435I was found.
[ INFO] [1730102770.648333256]: Device with port number 4-3 was found.
[ INFO] [1730102770.648430436]: Device USB type: 3.2
[ INFO] [1730102770.651161718]: getParameters...
[ INFO] [1730102770.671641606]: setupDevice...
[ INFO] [1730102770.671660994]: JSON file is not provided
[ INFO] [1730102770.671668310]: ROS Node Namespace: camera
[ INFO] [1730102770.671675578]: Device Name: Intel RealSense D435I
[ INFO] [1730102770.671682306]: Device Serial No: 332322073932
[ INFO] [1730102770.671688410]: Device physical port: 4-3-3
[ INFO] [1730102770.671706376]: Device FW version: 05.16.00.01
[ INFO] [1730102770.671718076]: Device Product ID: 0x0B3A
[ INFO] [1730102770.671724409]: Enable PointCloud: Off
[ INFO] [1730102770.671730529]: Align Depth: On
[ INFO] [1730102770.671738980]: Sync Mode: On
[ INFO] [1730102770.671784035]: Device Sensors: 
[ INFO] [1730102770.749818345]: Stereo Module was found.
[ INFO] [1730102770.757976075]: RGB Camera was found.
[ INFO] [1730102770.758093758]: Motion Module was found.
[ INFO] [1730102770.758124002]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1730102770.758353242]: num_filters: 1
[ INFO] [1730102770.758367938]: Setting Dynamic reconfig parameters.
 28/10 16:06:12,703 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
hwmon command 0x80( 5 0 0 0 ) failed (response -7= HW not ready)
hwmon command 0x80( 5 0 0 0 ) failed (response -7= HW not ready)
hwmon command 0x80( 5 0 0 0 ) failed (response -7= HW not ready)
hwmon command 0x80( 5 0 0 0 ) failed (response -7= HW not ready)
[ INFO] [1730102773.821387230]: Done Setting Dynamic reconfig parameters.
[ INFO] [1730102773.821711128]: depth stream is enabled - width: 640, height: 480, fps: 30, Format: Z16
[ INFO] [1730102773.821838551]: infra1 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1730102773.821974332]: infra2 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1730102773.822249294]: color stream is enabled - width: 640, height: 480, fps: 30, Format: RGB8
[ INFO] [1730102773.823609477]: gyro stream is enabled - fps: 200
[ INFO] [1730102773.823632515]: accel stream is enabled - fps: 200
[ INFO] [1730102773.823650244]: setupPublishers...
[ INFO] [1730102773.825180507]: Expected frequency for depth = 30.00000
[ INFO] [1730102773.843424917]: Expected frequency for infra1 = 30.00000
[ INFO] [1730102773.854231627]: Expected frequency for infra2 = 30.00000
[ INFO] [1730102773.864462827]: Expected frequency for color = 30.00000
[ INFO] [1730102773.875196168]: Expected frequency for aligned_depth_to_color = 30.00000
[ INFO] [1730102773.885408199]: Start publisher IMU
[ INFO] [1730102773.886349606]: setupStreams...
 28/10 16:06:14,019 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
 28/10 16:06:14,197 WARNING [140154711009024] (ds5-motion.cpp:473) IMU Calibration is not available, default intrinsic and extrinsic will be used.
 28/10 16:06:14,402 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ INFO] [1730102774.453459897]: SELECTED BASE:Depth, 0
[ INFO] [1730102774.475609974]: RealSense Node Is Up!
[ WARN] [1730102774.525907144]: 
 28/10 16:06:14,526 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
 28/10 16:06:14,577 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
 28/10 16:06:14,628 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
 28/10 16:06:14,679 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
 28/10 16:06:14,818 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ WARN] [1730102774.822353546]: Hardware Notification:Motion Module failure,1.7301e+12,Error,Hardware Error
 28/10 16:06:14,868 WARNING [140154685830912] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b

Is this part "Hardware Notification:Motion Module failure,1.7301e+12,Error,Hardware Error"what caused the problem? How can I solve this? Thanks !

dyb678 commented 3 weeks ago

image This is my Firmware information.

MartyG-RealSense commented 3 weeks ago

Hi @dyb678 It is not clear what causes a Motion Module Failure in the ROS wrapper when the camera is being accessed during launch.

Does it still happen if you reset the camera at launch by adding initial_reset:=true to your roslaunch instruction, please?

dyb678 commented 3 weeks ago

yep，it still cann‘t work. I consider if it is caused by the version of firmware. Could u plz tell me its firmware version and its corresponding realsense version？And，because I have downloaded 5.16 version，how can I downgrade the version.

dyb678 commented 3 weeks ago

It should be noted that it works in realsense viewer And I can check rgb，depth ，gyro and accel data.But in ros launch imu topic has blank data

MartyG-RealSense commented 3 weeks ago

Your current firmware is 5.16.0.1. The recommended firmware to use with librealsense 2.50.0 and ROS1 wrapper 2.3.2 is 5.13.0.50.

You can downgrade the firmware version to 5.13.0.50 in realsense-viewer using instructions at https://github.com/IntelRealSense/librealsense/issues/12599#issuecomment-1902680303

You should not make more than 20 firmware downgrade attempts though or the ability to downgrade permanently locks and you will only be able to upgrade to newer firmware versions than the one currently installed in your camera.

dyb678 commented 3 weeks ago

I have solved this problem and I can receive data.But I still have a problem: when I use “unite_imu_method = linear_interpolation” I find the imu data is not stable. Do you have the method to solve this?

MartyG-RealSense commented 3 weeks ago

Hi @dyb678 Setting unite_imu_method to copy instead of linear_interpolation can provide more stable IMU data.

MartyG-RealSense commented 1 week ago

Hi @dyb678 Do you require further assistance with this case, please? Thanks!

MartyG-RealSense commented 1 week ago

Case closed due to no further comments received.