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

opensource_tracking never publish imu data #2652

Closed Karin-Sugi closed 1 year ago

Karin-Sugi commented 1 year ago

Hi, to get orientation data of realsense D435i, I launched realsense2_camera opensource_tracking.launch with following the instraction.

Though I could launch? the file, both /imu/data & /camera/imu publish no message. I got following errors. But I think the error message is related rviz not imu, and based on this comment, the warning can be ignored.

I'm using ros melodic on virtualbox. I could get imu data using both viewer and realsense2_camera (here without orientation data). Also, I installed all packages: imu_filter_madgwick, rtabmap_ros, robot_localization. Also checked all package which imu_filter_madgwick depends are installed.

I think the imu_filter_madgwick pkg doesn't work properly, but have no idea any more... How can i fix it??

[ WARN] [1678680634.553167595]: Failed to meet update rate! Took 0.0069617060000000006645
[ WARN] [1678680634.564188656]: Failed to meet update rate! Took 0.0079884730000000011424
[ WARN] [1678680634.574119138]: Failed to meet update rate! Took 0.0079141160000000005531
[ERROR] [1678680634.582203146]: PluginlibFactory: The plugin for class 'octomap_rviz_plugin/ColorOccupancyGrid' failed to load.  Error: According to the loaded plugin descriptions the class octomap_rviz_plugin/ColorOccupancyGrid with base class type rviz::Display does not exist. Declared types are  rtabmap_ros/Info rtabmap_ros/MapCloud rtabmap_ros/MapGraph rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/WrenchStamped rviz_imu_plugin/Imu
[ WARN] [1678680634.600252167]: Failed to meet update rate! Took 0.0073689900000000006092
[ WARN] [1678680634.628478347]: Failed to meet update rate! Took 0.0089474820000000013925
[ WARN] [1678680634.661545837]: Failed to meet update rate! Took 0.0086810360000000013486
MartyG-RealSense commented 1 year ago

Have you enabled the IMU topics in your roslaunch instruction, please? At a later date after the D435i SLAM guide was published, the IMU topics were disabled by default in the ROS wrapper and so need to be enabled using the launch instruction below.

roslaunch realsense2_camera opensource_tracking.launch enable_gyro:=true enable_accel:=true unite_imu_method:=linear_interpolation

MartyG-RealSense commented 1 year ago

Hi @Karin-Sugi Do you require further assistance with this case, please? Thanks!

Karin-Sugi commented 1 year ago

@MartyG-RealSense Thanks for your help! I enabled the imu topic as you suggested, but it didn't work... Do you have any other idea?

Messages are below. 

[ INFO] [1679563457.173856842]: rtabmap (7): Rate=1.00s, Limit=0.000s, Conversion=0.0056s, RTAB-Map=0.1952s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[ WARN] [1679563457.182334080]: Failed to meet update rate! Took 0.02250095600000000251

Also, I list up all topics running. Please check all topics are exist.

$ rostopic list

/ImuFilter/parameter_descriptions
/ImuFilter/parameter_updates
/camera/align_to_color/parameter_descriptions
/camera/align_to_color/parameter_updates
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/aligned_depth_to_color/image_raw/compressed
/camera/aligned_depth_to_color/image_raw/compressed/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressed/parameter_updates
/camera/aligned_depth_to_color/image_raw/compressedDepth
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_updates
/camera/aligned_depth_to_color/image_raw/theora
/camera/aligned_depth_to_color/image_raw/theora/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/theora/parameter_updates
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/color/metadata
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/depth/metadata
/camera/extrinsics/depth_to_color
/camera/imu
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/disparity
/example/another_odom
/example/pose
/example/twist
/gps/fix
/imu/data
/initialpose
/left/camera_info
/left/image
/move_base_simple/goal
/odometry/filtered
/rgbd_image_relay
/right/camera_info
/right/image
/rosout
/rosout_agg
/rtabmap/cloud_ground
/rtabmap/cloud_map
/rtabmap/cloud_obstacles
/rtabmap/global_path
/rtabmap/global_path_nodes
/rtabmap/global_pose
/rtabmap/goal
/rtabmap/goal_node
/rtabmap/goal_out
/rtabmap/goal_reached
/rtabmap/grid_map
/rtabmap/grid_prob_map
/rtabmap/info
/rtabmap/initialpose
/rtabmap/labels
/rtabmap/landmarks
/rtabmap/local_grid_empty
/rtabmap/local_grid_ground
/rtabmap/local_grid_obstacle
/rtabmap/local_path
/rtabmap/local_path_nodes
/rtabmap/localization_pose
/rtabmap/mapData
/rtabmap/mapGraph
/rtabmap/mapOdomCache
/rtabmap/mapPath
/rtabmap/octomap_binary
/rtabmap/octomap_empty_space
/rtabmap/octomap_full
/rtabmap/octomap_global_frontier_space
/rtabmap/octomap_grid
/rtabmap/octomap_ground
/rtabmap/octomap_obstacles
/rtabmap/octomap_occupied_space
/rtabmap/odom
/rtabmap/odom_info
/rtabmap/odom_info_lite
/rtabmap/odom_last_frame
/rtabmap/odom_local_map
/rtabmap/odom_local_scan_map
/rtabmap/odom_rgbd_image
/rtabmap/proj_map
/rtabmap/republish_node_data
/rtabmap/scan_map
/set_pose
/tag_detections
/tf
/tf_static
/user_data_async
/voxel_cloud
MartyG-RealSense commented 1 year ago

When unite_imu_method is set then the two separate 'gyro' and 'accel' topics are combined into a single combined imu topic, which is present in the list as /camera/imu

In regard to the error of Failed to meet update rate!, there is very little documentation available on this error. When ROS does generate this error though, it sometimes provides this advice:

Try decreasing the rate, limiting sensor output frequency, or limiting the number of sensors.

I believe that 'sensor output frequency' is referring to the IMU frequency.

Karin-Sugi commented 1 year ago

Do you think limiting the imu frequency can be a solution, right? then, how can I reset the frequency? I'm sure i should change the number of parameter in launch file, but which one?

Thanks in advance!

MartyG-RealSense commented 1 year ago

You can set the IMU frequency in the launch instruction. The instruction will be different depending on whether your D435i was manufactured before or after mid 2022, so try both of the instructions below please.

Before mid 2022 roslaunch realsense2_camera opensource_tracking.launch enable_gyro:=true enable_accel:=true unite_imu_method:=linear_interpolation accel_fps:=63 gyro_fps:=200

After mid 2022 roslaunch realsense2_camera opensource_tracking.launch enable_gyro:=true enable_accel:=true unite_imu_method:=linear_interpolation accel_fps:=100 gyro_fps:=200

The ROS wrapper is likely already using these IMU speeds by default though. You could instead try changing unite_imu_method from linear_interpolation to copy as this can provide more stable IMU performance.

unite_imu_method:=copy

Karin-Sugi commented 1 year ago

Hi, I reset imu frequency with all method including unite_imu_method:=copy, but never work... No data is published on /camera/imu topic.

I'll show you messages in more detail. Do you have any other idea? Thanks in advance!

[ INFO] [1680059610.432962415]: Enable PointCloud: Off
[ INFO] [1680059610.433014447]: Align Depth: On
[ INFO] [1680059610.433212686]: Sync Mode: On
[ INFO] [1680059610.433250114]: Device Sensors: 
[ INFO] [1680059610.435476695]: Stereo Module was found.
[ INFO] [1680059610.440441332]: RGB Camera was found.
[ INFO] [1680059610.441134539]: Motion Module was found.
[ INFO] [1680059610.441406122]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1680059610.442055552]: num_filters: 1
[ INFO] [1680059610.442168565]: Setting Dynamic reconfig parameters.
[ INFO] [1680059610.445273766]: Approximate time sync = true
[ WARN] [1680059610.471562564]: Failed to meet update rate! Took 0.0075470630000000005558
[ WARN] [1680059610.504632372]: Failed to meet update rate! Took 0.010608850000000001265
[ WARN] [1680059610.521399644]: Failed to meet update rate! Took 0.023971053000000002581
[ WARN] [1680059610.523795143]: Failed to meet update rate! Took 0.016912089000000001676
[ INFO] [1680059610.533832943]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1680059610.533925136]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1680059610.535720036]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1680059610.535735095]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1680059610.535740486]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1680059610.535745362]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1680059610.535750701]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1680059610.535764735]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1680059610.537549241]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ WARN] [1680059610.538041991]: Failed to meet update rate! Took 0.01132308800000000025
[ WARN] [1680059610.539074434]: Failed to meet update rate! Took 0.0090329610000000008091
[ WARN] [1680059610.568174573]: Failed to meet update rate! Took 0.010072743999999999795
[ WARN] [1680059610.587634467]: Failed to meet update rate! Took 0.026214505000000002527
[ INFO] [1680059610.603617984]: Odometry: frame_id               = camera_link
[ INFO] [1680059610.603749785]: Odometry: odom_frame_id          = odom
[ INFO] [1680059610.603784161]: Odometry: publish_tf             = true
[ INFO] [1680059610.603802880]: Odometry: wait_for_transform     = true
[ INFO] [1680059610.603830114]: Odometry: wait_for_transform_duration  = 0.200000
[ INFO] [1680059610.603850979]: Odometry: log_to_rosout_level    = 4
[ INFO] [1680059610.603904862]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1680059610.603924332]: Odometry: ground_truth_frame_id  = 
[ INFO] [1680059610.603939596]: Odometry: ground_truth_base_frame_id = 
[ INFO] [1680059610.603947862]: Odometry: config_path            = 
[ INFO] [1680059610.603956084]: Odometry: publish_null_when_lost = true
[ INFO] [1680059610.604100755]: Odometry: guess_frame_id         = 
[ INFO] [1680059610.604123077]: Odometry: guess_min_translation  = 0.000000
[ INFO] [1680059610.604142309]: Odometry: guess_min_rotation     = 0.000000
[ INFO] [1680059610.604159915]: Odometry: guess_min_time         = 0.000000
[ INFO] [1680059610.604177266]: Odometry: expected_update_rate   = 0.000000 Hz
[ INFO] [1680059610.604196045]: Odometry: max_update_rate        = 0.000000 Hz
[ INFO] [1680059610.604214275]: Odometry: min_update_rate        = 0.000000 Hz
[ INFO] [1680059610.604234976]: Odometry: wait_imu_to_init       = false
[ INFO] [1680059610.604271303]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0
[ WARN] [1680059610.632405167]: Failed to meet update rate! Took 0.0079890220000000001371
[ WARN] [1680059610.636404779]: Failed to meet update rate! Took 0.0087953070000000004719
[ WARN] [1680059610.644175688]: Failed to meet update rate! Took 0.0077713670000000003787
[ WARN] [1680059610.659491938]: Failed to meet update rate! Took 0.0097587500000000000633
[ WARN] [1680059610.670049536]: Failed to meet update rate! Took 0.006985857000000000698
[ WARN] [1680059610.673925585]: Failed to meet update rate! Took 0.0075324700000000003111
[ WARN] [1680059610.701458971]: Failed to meet update rate! Took 0.0075429650000000004334
[ WARN] [1680059610.704451722]: Failed to meet update rate! Took 0.0072200340000000001739
[ WARN] [1680059610.740506018]: Failed to meet update rate! Took 0.01592573200000000147
[ WARN] [1680059610.740615315]: Failed to meet update rate! Took 0.012850503000000000967
[ WARN] [1680059610.747661304]: Failed to meet update rate! Took 0.0070496100000000004246
[ INFO] [1680059610.768314007]: rtabmap: frame_id      = camera_link
[ INFO] [1680059610.772515605]: rtabmap: map_frame_id  = map
[ INFO] [1680059610.772584462]: rtabmap: log_to_rosout_level = 4
[ INFO] [1680059610.772598633]: rtabmap: initial_pose  = 
[ INFO] [1680059610.772604388]: rtabmap: use_action_for_goal  = false
[ INFO] [1680059610.772614123]: rtabmap: tf_delay      = 0.050000
[ INFO] [1680059610.772625065]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1680059610.772631449]: rtabmap: odom_sensor_sync   = false
[ INFO] [1680059610.785149296]: rtabmap: gen_scan  = false
[ INFO] [1680059610.785233594]: rtabmap: gen_depth  = false
[ INFO] [1680059610.804264110]: Done Setting Dynamic reconfig parameters.
[ INFO] [1680059610.805120955]: depth stream is enabled - width: 848, height: 480, fps: 30, Format: Z16
[ WARN] [1680059610.830660314]: Failed to meet update rate! Took 0.0067071640000000007018
[ WARN] [1680059610.834105696]: Failed to meet update rate! Took 0.0066886820000000004813
[ INFO] [1680059610.834903627]: color stream is enabled - width: 1280, height: 720, fps: 30, Format: RGB8
[ INFO] [1680059610.836319805]: setupPublishers...
[ INFO] [1680059610.840250363]: Expected frequency for depth = 30.00000
[ WARN] [1680059610.855704132]: Failed to meet update rate! Took 0.0083912910000000003746
[ WARN] [1680059610.906397983]: Failed to meet update rate! Took 0.012437494000000000327
[ WARN] [1680059610.906803220]: Failed to meet update rate! Took 0.0095405720000000006353
[ WARN] [1680059610.944229017]: Failed to meet update rate! Took 0.013960554000000000019
[ WARN] [1680059610.946906668]: Failed to meet update rate! Took 0.013442160000000001607
[ WARN] [1680059611.000560818]: Failed to meet update rate! Took 0.015767061000000002352
[ WARN] [1680059611.000920174]: Failed to meet update rate! Took 0.014039791000000001117
[ WARN] [1680059611.018444940]: Failed to meet update rate! Took 0.0075180530000000002355
[ WARN] [1680059611.047959248]: Failed to meet update rate! Took 0.017021186000000000649
[ WARN] [1680059611.048860578]: Failed to meet update rate! Took 0.014540262000000001713
[ WARN] [1680059611.063131754]: Failed to meet update rate! Took 0.0077936430000000002719
[ INFO] [1680059611.097003083]: Expected frequency for color = 30.00000
[ INFO] [1680059611.107497970]: Expected frequency for aligned_depth_to_color = 30.00000
[ WARN] [1680059611.107940200]: Failed to meet update rate! Took 0.012606960000000000388
[ WARN] [1680059611.108025204]: Failed to meet update rate! Took 0.0093803600000000007808
[ WARN] [1680059611.131430005]: Failed to meet update rate! Took 0.0067228420000000005743
[ INFO] [1680059611.189298814]: setupStreams...
[ WARN] [1680059611.192114916]: Failed to meet update rate! Took 0.0074101240000000005714
[ WARN] [1680059611.227436229]: Failed to meet update rate! Took 0.0089012430000000013902
[ WARN] [1680059611.254951531]: Failed to meet update rate! Took 0.006910376000000000192
[ WARN] [1680059611.261121308]: Failed to meet update rate! Took 0.0097637840000000010454
[ WARN] [1680059611.283933861]: Failed to meet update rate! Took 0.0094634810000000010172
[ WARN] [1680059611.314429074]: Failed to meet update rate! Took 0.02329354100000000094
[ WARN] [1680059611.315499569]: Failed to meet update rate! Took 0.020959438000000000374
[ WARN] [1680059611.325796901]: Failed to meet update rate! Took 0.011088347000000000134
[ INFO] [1680059611.343981300]: SELECTED BASE:Depth, 0
[ WARN] [1680059611.353745786]: Failed to meet update rate! Took 0.035694688000000002193
[ WARN] [1680059611.363582478]: Failed to meet update rate! Took 0.0068561200000000002697
[ INFO] [1680059611.383256310]: RealSense Node Is Up!
[ WARN] [1680059611.390495461]: Failed to meet update rate! Took 0.0071137900000000000439
[ WARN] [1680059611.399464222]: Failed to meet update rate! Took 0.0094150870000000008025
[ WARN] [1680059611.410393934]: Failed to meet update rate! Took 0.0069776140000000000385
MartyG-RealSense commented 1 year ago

Failed to meet update rate is produced by robot_localization and not by librealsense or the RealSense ROS wrapper. I researched the subject extensively in non-RealSense discussions but it seems that nobody in the ROS community has found a solution to resolve the warnings despite many people having experienced them with robot_localization, unfortunately.

MartyG-RealSense commented 1 year ago

Hi @Karin-Sugi Do you require further assistance with this case, please? Thanks!

MartyG-RealSense commented 1 year ago

Case closed due to no further comments received.