introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.85k stars 787 forks source link

rviz crashes with MapCloud plugin #266

Open enwpoa332 opened 6 years ago

enwpoa332 commented 6 years ago

https://github.com/introlab/rtabmap/blob/129ec29af5c67113865cfc402b5c80d2d9a6639c/corelib/src/util3d_filtering.cpp#L183

Problem rviz terminates when attempting to subscribe to rtabmap_ros/MapData. Error message below: terminate called after throwing an instance of 'UException' what(): [FATAL] (2018-02-09 11:33:34.673) util3d_filtering.cpp:184::voxelize() Condition ((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())) not met! [Cloud size=19200 indices=0 is_dense=false] Aborted (core dumped)

Steps to reproduce

  1. Start roscore, rtabmap, dependent tasks (camera, odometry, etc)
  2. Start rviz
  3. In rviz,
    1. select "Add a new display"
    2. create the rtabmap_ros > MapCloud visualization
    3. In Displays > MapCloud > Topic, select /rtabmap/mapData rviz then exits and the above is shown

Context rviz version 1.12.15 rtabmap 0.15.0

I have been using rviz with RTABMap plugins to visualize for the past couple months. Today, in updating ros-kinetic, I found that the ROS OpenCV package was upgraded to version 3.3. I've since updated RTABMap, but it seems the rviz plugin for MapCloud now crashes rviz.

Thanks.

matlabbe commented 6 years ago

Hi,

It seems that a cloud has been created without any valid values (depth image is empty?), then passed to voxelize function. In devel branch, instead of asserting, an empty cloud is still returned with the same message but as a warning. On your current version, disable voxel filter in MapCloud display by setting "Cloud voxel size" to 0.

cheers, Mathieu