search

IntelRealSense / realsense-ros

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

D435i IMU Timming Problem #1756

Closed ThLi1 closed 3 years ago

ThLi1 commented 3 years ago

Hi, I used the imu module of d435i to record RosBag data on Raspberry Pi, but when I run cartographer_validate,it prompts that the imu data time is not timestamps are not strictly increasing and there is the large gap, the largest is 84.1733s. The system of Raspberry Pi is Raspbian-buster. When I try to connect d435i with my computer to record the RosBag, there will be no such problem. Had anyone the same problem as me? How did you solve it? Thanks!

frame_id "laser" on topic /scan/ has serialization time 1615285593.212962558 but sensor time 1615285593.111784799 differing by -0.101178 s. E0313 10:22:06.531116 4499 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu/ has serialization time 1615285596.116480667 but sensor time 1615284828.469705105 differing by -767.647 s. E0313 10:22:06.531322 4499 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu/ has serialization time 1615285596.119050390 but sensor time 1615284828.471746445 differing by -767.647 s. E0313 10:22:06.531373 4499 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu/ has serialization time 1615285596.121465001 but sensor time 1615284828.475828886 differing by -767.646 s. E0313 10:22:06.564853 4499 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0313 10:22:06.568487 4499 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0313 10:22:06.571218 4499 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. W0313 10:22:06.616080 4499 rosbag_validate_main.cc:103] frame_id camera_imu_optical_frame time 1615285082095139265: IMU linear acceleration is 30.5092 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 03.34937 -29.5105 -6.98031 W0313 10:22:06.616112 4499 rosbag_validate_main.cc:103] frame_id camera_imu_optical_frame time 1615285082096672773: IMU linear acceleration is 30.5092 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 03.34937 -29.5105 -6.98031 W0313 10:22:06.649425 4499 rosbag_validate_main.cc:103] frame_id camera_imu_optical_frame time 1615285111511512995: IMU linear acceleration is 2.10424 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 001.86501 -0.938739 00.261361 E0313 10:22:06.763056 4499 rosbag_validate_main.cc:390] IMU data (frame_id: "camera_imu_optical_frame") has a large gap, largest is 84.1733 s, recommended is [0.0005, 0.005] s with no jitter. I0313 10:22:06.763733 4499 rosbag_validate_main.cc:399] Time delta histogram for consecutive messages on topic "/camera/imu/" (frame_id: "camera_imu_optical_frame"): Count: 76779 Min: -0.0260808 Max: 84.1733 Mean: 0.00499224 [-0.026081, 8.393857) #################### Count: 76775 (99.9948%) Total: 76775 (99.9948%) [8.393857, 16.813795) Count: 1 (0.00130244%) Total: 76776 (99.9961%) [16.813795, 25.233732) Count: 1 (0.00130244%) Total: 76777 (99.9974%) [25.233732, 33.653671) Count: 0 (0%) Total: 76777 (99.9974%) [33.653671, 42.073612) Count: 1 (0.00130244%) Total: 76778 (99.9987%) [42.073612, 50.493546) Count: 0 (0%) Total: 76778 (99.9987%) [50.493546, 58.913490) Count: 0 (0%) Total: 76778 (99.9987%) [58.913490, 67.333420) Count: 0 (0%) Total: 76778 (99.9987%) [67.333420, 75.753357) Count: 0 (0%) Total: 76778 (99.9987%) [75.753357, 84.173302] Count: 1 (0.00130244%) Total: 76779 (100%) E0313 10:22:06.764138 4499 rosbag_validate_main.cc:383] Point data (frame_id: "laser") has a large gap, largest is 0.319597 s, recommended is [0.0005, 0.05] s with no jitter. I0313 10:22:06.764158 4499 rosbag_validate_main.cc:399] Time delta histogram for consecutive messages on topic "/scan/" (frame_id: "laser"): Count: 2182 Min: 0.0694106 Max: 0.319597 Mean: 0.089423 [0.069411, 0.094429) ######## Count: 865 (39.6425%) Total: 865 (39.6425%) [0.094429, 0.119448) ############ Count: 1307 (59.8992%) Total: 2172 (99.5417%) [0.119448, 0.144466) Count: 1 (0.0458295%) Total: 2173 (99.5875%) [0.144466, 0.169485) Count: 0 (0%) Total: 2173 (99.5875%) [0.169485, 0.194504) Count: 0 (0%) Total: 2173 (99.5875%) [0.194504, 0.219522) Count: 1 (0.0458295%) Total: 2174 (99.6334%) [0.219522, 0.244541) Count: 0 (0%) Total: 2174 (99.6334%) [0.244541, 0.269560) Count: 0 (0%) Total: 2174 (99.6334%) [0.269560, 0.294578) Count: 1 (0.0458295%) Total: 2175 (99.6792%) [0.294578, 0.319597] Count: 7 (0.320807%) Total: 2182 (100%)

MartyG-RealSense commented 3 years ago

Hi @ThLi1 A large drift in IMU timestamp values can occur if global time is enabled. Please try adding the instruction below to the end of your roslaunch instruction to disable global time:

global_time_enabled:=false

In some cases, adding to the roslaunch instruction both this command and also initial_reset:=true to reset the camera at launch are required to correct the problem.

ThLi1 commented 3 years ago

Hi @ThLi1 A large drift in IMU timestamp values can occur if global time is enabled. Please try adding the instruction below to the end of your roslaunch instruction to disable global time:

global_time_enabled:=false

In some cases, adding to the roslaunch instruction both this command and also initial_reset:=true to reset the camera at launch are required to correct the problem.

Hi @MartyG-RealSense , thanks for your answer. I tried your method, but there is still a lot of time drift. But I don't know if I understand you correctly.

I tried to type roslaunch realsense2_camera imu.launch global_time_enabled:=false initial_reset:=true

MartyG-RealSense commented 3 years ago

Do you have improved results if you use the roslaunch instruction below please?

roslaunch realsense2_camera rs_camera.launch enable_gyro:=true enable_accel:=true global_time_enabled:=false initial_reset:=true

ThLi1 commented 3 years ago

Do you have improved results if you use the roslaunch instruction below please?

roslaunch realsense2_camera rs_camera.launch enable_gyro:=true enable_accel:=true global_time_enabled:=false initial_reset:=true

I think so, after I use cartographer validate, the result looks better and the largest gap is reduced to 0.7s. But the difference between the Serialization time and the sensor time is -582.112 s.

The result of Cartographer_validate: ` cartographer_rosbag_validate -bag_filename /home/taihao/RTL_bag/scan_imu_n.bag E0318 12:15:59.954506 24760 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu/ has serialization time 1615295406.756764490 but sensor time 1615294824.644804001 differing by -582.112 s. E0318 12:15:59.954795 24760 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu/ has serialization time 1615295406.759310656 but sensor time 1615294824.806287050 differing by -581.953 s. E0318 12:15:59.954810 24760 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu/ has serialization time 1615295406.761755490 but sensor time 1615294824.807834387 differing by -581.954 s. W0318 12:15:59.956162 24760 rosbag_validate_main.cc:353] frame_id "laser" on topic /scan/ has serialization time 1615295407.804394896 but sensor time 1615295407.697873619 differing by -0.106521 s. E0318 12:15:59.968312 24760 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0318 12:15:59.970080 24760 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0318 12:15:59.971232 24760 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. W0318 12:15:59.997071 24760 rosbag_validate_main.cc:103] frame_id camera_imu_optical_frame time 1615294863892977476: IMU linear acceleration is 37.6826 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 0.966751 -31.2589 -21.0221 W0318 12:15:59.997210 24760 rosbag_validate_main.cc:103] frame_id camera_imu_optical_frame time 1615294864003649473: IMU linear acceleration is 32.5373 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 02.11733 -25.9226 -19.5502 W0318 12:15:59.997227 24760 rosbag_validate_main.cc:103] frame_id camera_imu_optical_frame time 1615294864005186558: IMU linear acceleration is 32.5373 m/s^2, expected is [3, 30] m/s^2. (It should include gravity and be given in m/s^2.) linear_acceleration 02.11733 -25.9226 -19.5502 E0318 12:16:00.060417 24760 rosbag_validate_main.cc:390] IMU data (frame_id: "camera_imu_optical_frame") has a large gap, largest is 0.760743 s, recommended is [0.0005, 0.005] s with no jitter. I0318 12:16:00.060544 24760 rosbag_validate_main.cc:399] Time delta histogram for consecutive messages on topic "/camera/imu/" (frame_id: "camera_imu_optical_frame"):

Count: 31632 Min: -0.688794 Max: 0.760743 Mean: 0.00267965 [-0.688794, -0.543840) Count: 3 (0.00948407%) Total: 3 (0.00948407%) [-0.543840, -0.398886) Count: 2 (0.00632271%) Total: 5 (0.0158068%) [-0.398886, -0.253933) Count: 9 (0.0284522%) Total: 14 (0.044259%) [-0.253933, -0.108979) Count: 11 (0.0347749%) Total: 25 (0.0790339%) [-0.108979, 0.035975) #################### Count: 31565 (99.7882%) Total: 31590 (99.8672%) [0.035975, 0.180928) Count: 12 (0.0379363%) Total: 31602 (99.9052%) [0.180928, 0.325882) Count: 11 (0.0347749%) Total: 31613 (99.9399%) [0.325882, 0.470836) Count: 12 (0.0379363%) Total: 31625 (99.9779%) [0.470836, 0.615790) Count: 4 (0.0126454%) Total: 31629 (99.9905%) [0.615790, 0.760743] Count: 3 (0.00948407%) Total: 31632 (100%) `

MartyG-RealSense commented 3 years ago

Thanks very much for the information. Do you see any further improvement if you use unite_imu_method in your roslaunch to unite the gyro and accel topics under a single imu topic? The imu topic is published at the rate of the gyro.

roslaunch realsense2_camera rs_camera.launch enable_gyro:=true enable_accel:=true global_time_enabled:=false initial_reset:=true unite_imu_method:=linear_interpolation

ThLi1 commented 3 years ago

Thanks very much for the information. Do you see any further improvement if you use unite_imu_method in your roslaunch to unite the gyro and accel topics under a single imu topic? The imu topic is published at the rate of the gyro.

roslaunch realsense2_camera rs_camera.launch enable_gyro:=true enable_accel:=true global_time_enabled:=false initial_reset:=true unite_imu_method:=linear_interpolation

Thanks very much for the answer, I tried this command, but the result is worse. The method I used before was "copy" and I can get the better result.

new result (when I use "linear_interpolation") ` E0318 13:20:17.595793 27441 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu has serialization time 1615296766.135609677 but sensor time 1615295583.249030828 differing by -1182.89 s. E0318 13:20:17.596073 27441 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu has serialization time 1615296766.135672232 but sensor time 1615295583.252135038 differing by -1182.88 s. E0318 13:20:17.596112 27441 rosbag_validate_main.cc:351] frame_id "camera_imu_optical_frame" on topic /camera/imu has serialization time 1615296766.135690955 but sensor time 1615295583.255239248 differing by -1182.88 s. E0318 13:20:17.596125 27441 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0318 13:20:17.598824 27441 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0318 13:20:17.603523 27441 rosbag_validate_main.cc:316] Sensor with frame_id "camera_imu_optical_frame" jumps backwards in time, i.e. timestamps are not strictly increasing. Make sure that the bag contains the data for each frame_id sorted by header.stamp, i.e. the order in which they were acquired from the sensor. E0318 13:20:17.617671 27441 rosbag_validate_main.cc:390] IMU data (frame_id: "camera_imu_optical_frame") has a large gap, largest is 35.2483 s, recommended is [0.0005, 0.005] s with no jitter. I0318 13:20:17.617727 27441 rosbag_validate_main.cc:399] Time delta histogram for consecutive messages on topic "/camera/imu" (frame_id: "camera_imu_optical_frame"):

Count: 8906 Min: -48.6006 Max: 35.2483 Mean: -0.00856938 [-48.600571, -40.215683) Count: 1 (0.0112284%) Total: 1 (0.0112284%) [-40.215683, -31.830786) Count: 3 (0.0336852%) Total: 4 (0.0449135%) [-31.830786, -23.445892) Count: 2 (0.0224568%) Total: 6 (0.0673703%) [-23.445892, -15.061004) Count: 1 (0.0112284%) Total: 7 (0.0785987%) [-15.061004, -6.676111) Count: 1 (0.0112284%) Total: 8 (0.0898271%) [-6.676111, 1.708780) #################### Count: 8891 (99.8316%) Total: 8899 (99.9214%) [1.708780, 10.093672) Count: 2 (0.0224568%) Total: 8901 (99.9439%) [10.093672, 18.478565) Count: 1 (0.0112284%) Total: 8902 (99.9551%) [18.478565, 26.863457) Count: 3 (0.0336852%) Total: 8905 (99.9888%) [26.863457, 35.248348] Count: 1 (0.0112284%) Total: 8906 (100%) `

MartyG-RealSense commented 3 years ago

I'm not certain if this will be relevant to your particular application (it involves robot transforms). I recall a discussion where if the timestamps had a minus value (i.e they were in the future) then they could be corrected by "using ros::Time::now() instead of ros::Time(0) in your waitForTransform to get the transform at the current time instead of the latest transform".

https://answers.ros.org/question/331027/realsense-d435-lookup-would-require-extrapolation-into-the-future/?answer=331028#post-id-331028

ThLi1 commented 3 years ago

I'm not certain if this will be relevant to your particular application (it involves robot transforms). I recall a discussion where if the timestamps had a minus value (i.e they were in the future) then they could be corrected by "using ros::Time::now() instead of ros::Time(0) in your waitForTransform to get the transform at the current time instead of the latest transform".

https://answers.ros.org/question/331027/realsense-d435-lookup-would-require-extrapolation-into-the-future/?answer=331028#post-id-331028

Thanks for your advice, sorry for the dumb question, Where can i modify this code?

MartyG-RealSense commented 3 years ago

It is not a dumb question at all. :)

The ROS wiki has a tutorial for these time transform functions.

Python http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28Python%29

C++ http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C%2B%2B%29

These tutorials rely on a certain amount of pre-existing knowledge gained from earlier tutorials though. So this approach may not be the best one to follow unless you are confident in ROS programming.

ThLi1 commented 3 years ago

It is not a dumb question at all. :)

The ROS wiki has a tutorial for these time transform functions.

Python http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28Python%29

C++ http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C%2B%2B%29

These tutorials rely on a certain amount of pre-existing knowledge gained from earlier tutorials though. So this approach may not be the best one to follow unless you are confident in ROS programming.

Thanks for your advice! I'm sorry I was busy lately and didn't reply in time. But I'm also confused why this problem only occurs on the Raspberry Pi. When I used my computer with Ubuntu 18.04 to record imu data, there are no errors.

MartyG-RealSense commented 3 years ago

What method do you use to record the bag please (RealSense Viewer or rosbag record in ROS).

Which IMU frequencies are you recording at on your Pi? 200 gyro 63 accelerometer, or 400 gyro 250 accelerometer?

ThLi1 commented 3 years ago

What method do you use to record the bag please (RealSense Viewer or rosbag record in ROS).

Which IMU frequencies are you recording at on your Pi? 200 gyro 63 accelerometer, or 400 gyro 250 accelerometer?

I used rosbag record in ROS. The IMU frequencies are 400 gyro 250 accelerometer.

MartyG-RealSense commented 3 years ago

It may be worth reducing to 200 / 63 if that is practical for your project, bearing in mind that it is running on a Pi (Pi 4?) instead of a full PC.

MartyG-RealSense commented 3 years ago

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

ThLi1 commented 3 years ago

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

Hi, thanks, I tried to set 200/63 but can't solve the problem. Now I try to use ubuntu System but I have not yet verified.

MartyG-RealSense commented 3 years ago

Thanks very much for the update. I will keep this case open for a further time period. Good luck with the testing!

MartyG-RealSense commented 3 years ago

Hi @ThLi1 Do you have an update about this case that you can provide, please? Thanks!

MartyG-RealSense commented 3 years ago

Case closed due to no further comments received.

tomy983 commented 2 years ago

Hello, Here is what I get as Tf error in Rviz:

For frame [camera_imu_optical_frame]: No transform to fixed frame [camera_link]. TF error: [Lookup would require extrapolation -1221.571560145s into the future. Requested time 1654955072.998750687 but the latest data is at time 1654953851.427190542, when looking up transform from frame [camera_imu_optical_frame] to frame [camera_link]]

My launch file (the camera part) is as follow (added global_time and initial_reset parameters to my nodelet.launch.xml :

<launch>
<arg name="offline"          default="false"/>
    <include unless="$(arg offline)" 
        file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="camera"              default="camera"/>
<arg name="tf_prefix"           default="camera"/>
<arg name="global_time_enabled" default="false"/>
<arg name="initial_reset"       default="true"/>
<arg name="depth_width"         default="640"/>
<arg name="depth_height"        default="480"/>
<arg name="enable_color"        default="false"/>
<arg name="depth_fps"           default="6"/>
<arg name="enable_gyro"         default="true"/>
<arg name="enable_accel"        default="true"/>
<arg name="unite_imu_method"    default="linear_interpolation"/> <!-- Options are: [none, copy, linear_interpolation] -->  
</include>

<node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
        <param name="use_mag" type="bool" value="false" />
        <param name="_publish_tf" type="bool" value="false" />
        <param name="_world_frame" type="string" value="enu" />
        <remap from="/imu/data_raw" to="/camera/imu"/>
</node>
</launch>

I am running on a raspberry pi 4 8gb (arm64), ros noetic on ubuntu server 20.04 I compiled from source the most recent librealsense following this guide: https://answers.ros.org/question/363889/intel-realsens-on-ubuntu-2004-ros-noetic-installation-desription/

I had several issues, but reducing framerate, getting rid of color and pointclout etc.. I now can get a kind of stable streaming of depth image and imu (that's all I want for now)

Typical output:
[ INFO] [1654956683.285063234]:  
 11/06 16:11:23,389 WARNING [281472392999280] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ INFO] [1654956683.693033760]: Device with serial number 034422074202 was found.

[ INFO] [1654956683.693234962]: Device with physical ID 2-1-23 was found.
[ INFO] [1654956683.693516453]: Device with name Intel RealSense D435I was found.
[ INFO] [1654956683.694529259]: Device with port number 2-1 was found.
[ INFO] [1654956683.694656450]: Device USB type: 3.2
[ INFO] [1654956683.703527175]: getParameters...
[ INFO] [1654956684.298039436]: setupDevice...
[ INFO] [1654956684.298180570]: JSON file is not provided
[ INFO] [1654956684.298239249]: ROS Node Namespace: camera
[ INFO] [1654956684.298298021]: Device Name: Intel RealSense D435I
[ INFO] [1654956684.298351275]: Device Serial No: 034422074202
[ INFO] [1654956684.298398826]: Device physical port: 2-1-23
[ INFO] [1654956684.298453006]: Device FW version: 05.13.00.50
[ INFO] [1654956684.298500668]: Device Product ID: 0x0B3A
[ INFO] [1654956684.298544922]: Enable PointCloud: Off
[ INFO] [1654956684.298586715]: Align Depth: Off
[ INFO] [1654956684.298628062]: Sync Mode: Off
[ INFO] [1654956684.298755568]: Device Sensors: 
[ WARN] [1654956684.411974424]: Still waiting for data on topic /imu/data_raw...
[ INFO] [1654956684.450722739]: Stereo Module was found.
[ INFO] [1654956684.628158217]: RGB Camera was found.
[ INFO] [1654956684.629004984]: Motion Module was found.
[ INFO] [1654956684.629233369]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1654956684.642086900]: num_filters: 0
[ INFO] [1654956684.642264993]: Setting Dynamic reconfig parameters.
[ INFO] [1654956688.387092868]: Done Setting Dynamic reconfig parameters.
[ INFO] [1654956688.388872913]: depth stream is enabled - width: 848, height: 480, fps: 6, Format: Z16
[ INFO] [1654956688.389135849]: gyro stream is enabled - fps: 200
[ INFO] [1654956688.389241987]: accel stream is enabled - fps: 63
[ INFO] [1654956688.390806423]: setupPublishers...
[ INFO] [1654956688.410419407]: Expected frequency for depth = 6.00000
[ INFO] [1654956688.688520855]: Start publisher IMU
[ INFO] [1654956688.692347157]: setupStreams...
 11/06 16:11:28,871 WARNING [281472824828272] (ds5-motion.cpp:473) IMU Calibration is not available, default intrinsic and extrinsic will be used.
[ WARN] [1654956689.149534891]: 
[ WARN] [1654956689.151593888]: frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically.
[ WARN] [1654956689.211422446]: Transform from camera_imu_optical_frame to base_link was unavailable for the time requested. Using latest instead.

[ WARN] [1654956689.215295651]: Transform from camera_imu_optical_frame to base_link was unavailable for the time requested. Using latest instead.

[ WARN] [1654956689.218378102]: Transform from camera_imu_optical_frame to base_link was unavailable for the time requested. Using latest instead.

 11/06 16:11:29,309 WARNING [281472392999280] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ INFO] [1654956689.366619748]: SELECTED BASE:Depth, 0
[ WARN] [1654956689.395220168]: Hardware Notification:Motion Module failure,1.65496e+12,Error,Hardware Error
[ WARN] [1654956689.407339290]: Failed to meet update rate! Took 0.14878538700000001893
[ WARN] [1654956689.420963039]: Failed to meet update rate! Took 0.14577119700000001901
[ WARN] [1654956689.492981415]: Could not obtain transform from camera_imu_optical_frame to base_link. Error was Could not find a connection between 'base_link' and 'camera_imu_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.

[ WARN] [1654956689.501459271]: Could not obtain transform from camera_imu_optical_frame to base_link. Error was Could not find a connection between 'base_link' and 'camera_imu_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.

[ WARN] [1654956689.502578289]: Could not obtain transform from camera_imu_optical_frame to base_link. Error was Could not find a connection between 'base_link' and 'camera_imu_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.

[ INFO] [1654956689.534037052]: RealSense Node Is Up!
[ WARN] [1654956689.538276184]: Failed to meet update rate! Took 0.2464317220000000197
[ WARN] [1654956689.562098525]: Failed to meet update rate! Took 0.25373216800000003568
[ WARN] [1654956689.651319089]: Failed to meet update rate! Took 0.24315952900000001313
[ WARN] [1654956689.658620841]: Failed to meet update rate! Took 0.2306548160000000125
 11/06 16:11:29,738 WARNING [281472392999280] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11

The error [ WARN] [1654956689.395220168]: Hardware Notification:Motion Module failure,1.65496e+12,Error,Hardware Error it is the first time I noticed it, and in the realsense-viewer it works fine. Looks like it was just this time it happened.

I did see raccomandation for changing the way the Tf catch the error like: https://answers.ros.org/question/331027/realsense-d435-lookup-would-require-extrapolation-into-the-future/ but I really dont't know where to look for that part to change. Is it in the realsense-ros package? How could I trie to fix this issue?

Thank you for your help

MartyG-RealSense commented 2 years ago

Hi @tomy983 I researched your question carefully but there was not a clear answer to be found. The impression that I get is that the solution about setting up a catch for the exception does not involve editing the RealSense ROS wrapper but instead writing a TF listener script. ROS has a couple of official tutorials for doing this in C++ or Python.

C++ http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29

Python http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29

I would recommend instead trying setting unite_imu_method in your launch file to copy instead of linear_interpolation, as copy should have more stable IMU messages than linear_interpolation.

tomy983 commented 2 years ago

Thanks @MartyG-RealSense, I kept troubleshooting, and now the timing error is much reduced, but still to much (and sometimes missing):

[ WARN] [1655022909.970841326]: Failed to meet update rate! Took 0.11518427900000000053
[ WARN] [1655022909.984915343]: Failed to meet update rate! Took 0.12723439100000000224
[ WARN] [1655022910.026475939]: Failed to meet update rate! Took 0.092346377000000007262
[ WARN] [1655022910.045774961]: Transform from camera_imu_optical_frame to base_link was unavailable for the time requested. Using latest instead.

[ WARN] [1655022910.051699381]: Failed to meet update rate! Took 0.16269560800000001932
[ WARN] [1655022910.077933367]: Failed to meet update rate! Took 0.11062550900000001075
[ WARN] [1655022910.103826287]: Failed to meet update rate! Took 0.13066209100000000798

I am now using the copy method instead of linear_interpolation , even tough it does not make a lot of difference. What actually changed a lot of the time issue was the fact that I was running without a urdf model (now it is set). Adding that and setting the <param name="fixed_frame" type="string" value="base_link" /> for the imu_filter_madgwick reduced the timing difference from thousands of seconds to decimal of seconds.

Still the error persist. And, what is worst, after a while, the stream of the imu stops. I've made a short ad terrible video about what is going on. on the top left you can see the /imu/data at the top and the /camera/imu .

https://youtu.be/vAkBIy_u-Gc

The present settings are as follows:

<arg name="initial_reset"       default="true"/>
<arg name="depth_width"         default="848"/>
<arg name="depth_height"        default="480"/>
<arg name="enable_color"        default="false"/>
<arg name="depth_fps"           default="6"/>
<arg name="enable_gyro"         default="true"/>
<arg name="enable_accel"        default="true"/>
<arg name="unite_imu_method"    default="copy"/> <!-- Options are: [none, copy, linear_interpolation] -->
<arg name="enable_sync"               default="true"/>

I've tried to enable sync, makes really no difference...

And here are the startup sequence:

 INFO] [1655023831.251090598]: Device FW version: 05.13.00.50
[ INFO] [1655023831.251169377]: Device Product ID: 0x0B3A
[ INFO] [1655023831.251230615]: Enable PointCloud: Off
[ INFO] [1655023831.251311488]: Align Depth: Off
[ INFO] [1655023831.251567738]: Sync Mode: On
[ INFO] [1655023831.251734688]: Device Sensors: 
[ INFO] [1655023831.439074479]: Stereo Module was found.
[ INFO] [1655023831.559330850]: RGB Camera was found.
[ INFO] [1655023831.560597543]: Motion Module was found.
[ INFO] [1655023831.561292646]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1655023831.561743742]: num_filters: 0
[ INFO] [1655023831.562228310]: Setting Dynamic reconfig parameters.
[ INFO] [1655023835.346232472]: Done Setting Dynamic reconfig parameters.
[ INFO] [1655023835.348072578]: depth stream is enabled - width: 848, height: 480, fps: 6, Format: Z16
[ INFO] [1655023835.348324586]: gyro stream is enabled - fps: 200
[ INFO] [1655023835.348385156]: accel stream is enabled - fps: 63
[ INFO] [1655023835.348445431]: setupPublishers...
[ INFO] [1655023835.362000272]: Expected frequency for depth = 6.00000
[ INFO] [1655023835.732213387]: Start publisher IMU
[ INFO] [1655023835.736838076]: setupStreams...
 12/06 10:50:35,988 WARNING [281472921002352] (ds5-motion.cpp:473) IMU Calibration is not available, default intrinsic and extrinsic will be used.
[ WARN] [1655023836.348609964]: 
[ WARN] [1655023836.348766262]: frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically.
 12/06 10:50:36,347 WARNING [281472476893552] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ WARN] [1655023836.411614174]: Failed to meet update rate! Took 0.07249568500000000415

I am now trying to run with separated imu sensors. The data flow is much more stable, both sensors rarely have the issue of timing in the future and looks like they keep running. The Failed to meet update rate! error never occurs.

What is going on with the sensors union method? Are you aware of any packages to unite the sensors before to feed them to the filter?

MartyG-RealSense commented 2 years ago

Is there a positive difference if you change the IMU frequencies by adding the commands below to your roslaunch instruction?

accel_fps:=250 gyro_fps:=400

tomy983 commented 2 years ago

Will check. I just edited my previous comment at the end.

tomy983 commented 2 years ago

Is there a positive difference if you change the IMU frequencies by adding the commands below to your roslaunch instruction?

accel_fps:=250 gyro_fps:=400

Not really. The timing difference seems a bit higher, but the stream still stopped after few thousand messages of the /imu/data topic..

running with: accel_fps:=250 gyro_fps:=400

[ WARN] [1655026743.522380486]: Failed to meet update rate! Took 0.12341680700000000337
[ WARN] [1655026743.579138768]: Failed to meet update rate! Took 0.27112130700000003358
[ WARN] [1655026743.617561126]: Failed to meet update rate! Took 0.18525016700000002112
[ WARN] [1655026743.666357124]: Failed to meet update rate! Took 0.18189324300000001
[ WARN] [1655026743.698745086]: Failed to meet update rate! Took 0.17484439800000001197
[ WARN] [1655026743.742114992]: Failed to meet update rate! Took 0.16168847799999999659
[ WARN] [1655026743.745647444]: Failed to meet update rate! Took 0.12695629699999999573
[ WARN] [1655026743.802374091]: Failed to meet update rate! Took 0.13493884200000000329
[ WARN] [1655026743.814129361]: Failed to meet update rate! Took 0.11426990400000000547
[ WARN] [1655026743.819278139]: Transform from camera_imu_optical_frame to base_link was unavailable for the time requested. Using latest instead.

[ WARN] [1655026743.836130994]: Failed to meet update rate! Took 0.093047521000000008029
[ WARN] [1655026743.850625443]: Failed to meet update rate! Took 0.11742141100000000331
[ WARN] [1655026743.899303167]: Failed to meet update rate! Took 0.1228466520000000145
[ WARN] [1655026743.904844961]: Failed to meet update rate! Took 0.089680055000000008514
MartyG-RealSense commented 2 years ago

The RealSense ROS team member who developed the ROS1 wrapper gave an explanation of copy vs linear_interpolation at https://github.com/IntelRealSense/realsense-ros/issues/729#issuecomment-525623221

Some RealSense users have tried using VINS-Fusion with their IMU-equipped cameras.

https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

Someone else at https://github.com/IntelRealSense/realsense-ros/issues/1932#issuecomment-864157132 used the imu_tools ROS package

tomy983 commented 2 years ago

Tank you very much @MartyG-RealSense. This might be useful. Unfortunately, after a while, I am also loosing the camera/accel/sample and camera/gyro/sample. They just stop streaming. Depth image is still coming regularly. Imu data, in any way I set it, it just stops working. Any suggestions? Anything I can check?

MartyG-RealSense commented 2 years ago

I found a RealSense ROS user with a Raspberry Pi (like you) who reported an IMU stop problem at https://github.com/IntelRealSense/realsense-ros/issues/1469#issuecomment-727620915

I recommend reading the comments below the linked-to point in that discussion too.

The issue relates to the ROS1 wrapper, and the RealSense ROS development team focus is now on the ros2_beta wrapper. To the best of my knowledge, further updates to the ROS1 wrapper are not planned.

A RealSense ROS user at https://github.com/IntelRealSense/realsense-ros/issues/1469#issuecomment-719533272 (not a Pi user though) resolved their particular IMU freeze problem with the settings below.

arg name="gyro_fps" default="200"
arg name="accel_fps" default="63"
tomy983 commented 2 years ago

Thanks @MartyG-RealSense, I've looked at those links. Unfortunately, they do not solve my freezing imu.. I probably tried every combination of setting possible including the dynamic settings (frames queue size, motion correction ecc). Is there anything else I could try? Could an older librealsense library version work? Any way to restart the motion module when I see no more topics coming or other workaround?

MartyG-RealSense commented 2 years ago

Sometimes using an older librealsense version does make a difference. For example, Nvidia Jetson boards specifically have problems with pointcloud or depth-color alignment generation in the RealSense ROS wrapper that tend to not occur if using the older 2.43.0 SDK.

Conceivably, if you are able to detect that there are no messages from the /imu topic then you could use a ROS node script to reset the entire camera in the same way that the ROS wrapper does so with the initial_reset parameter. The wrapper's realsense_node_factory.cpp script provides an example of such code.

https://github.com/IntelRealSense/realsense-ros/blob/development/realsense2_camera/src/realsense_node_factory.cpp#L207-L212

Alternatively, the RealSense ROS2 wrapper has the ability to enable and disable individual sensors, during runtime with <stream>.enable though the ROS1 wrapper does not.