search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
978 stars 557 forks source link

simulated velodyne point cloud causing Segmentation fault (core dumped) on rtabmap/icp_odometry nodes #560

Open matlabbe opened 3 years ago

matlabbe commented 3 years ago

Here is a backtrace from gdb:

thread 29 "icp_odometry" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffb0d25700 (LWP 154167)]
__memmove_avx_unaligned_erms () at ../sysdeps/x86_64/multiarch/memmove-vec-unaligned-erms.S:312
312 ../sysdeps/x86_64/multiarch/memmove-vec-unaligned-erms.S: No such file or directory.
(gdb) bt
#0  __memmove_avx_unaligned_erms () at ../sysdeps/x86_64/multiarch/memmove-vec-unaligned-erms.S:312
#1  0x00007fffb0314245 in void pcl::fromPCLPointCloud2<pcl::PointXYZI>(pcl::PCLPointCloud2 const&, pcl::PointCloud<pcl::PointXYZI>&, std::vector<pcl::detail::FieldMapping, std::allocator<pcl::detail::FieldMapping> > const&) ()
    at /home/mathieu/catkin_ws/devel/lib//librtabmap_plugins.so
#2  0x00007fffb031b420 in void pcl::fromROSMsg<pcl::PointXYZI>(sensor_msgs::PointCloud2_<std::allocator<void> > const&, pcl::PointCloud<pcl::PointXYZI>&) () at /home/mathieu/catkin_ws/devel/lib//librtabmap_plugins.so
#3  0x00007fffb032247c in rtabmap_ros::ICPOdometry::callbackCloud(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&) () at /home/mathieu/catkin_ws/devel/lib//librtabmap_plugins.so

The bug happens when converting the PointCloud2 message in PCL templated cloud version. It is crashing here.

The point cloud width/height/row_step are wrong. This bug seems coming from this commit in velodyne gazebo plugin. A solution is to invert the width and height values here to :

    msg.width = msg.data.size() / POINT_STEP;
    msg.height = 1;
matlabbe commented 3 years ago

The issue reported upstream: https://bitbucket.org/DataspeedInc/velodyne_simulator/issues/26/inverted-cloud-width-and-height-causing

matlabbe commented 3 years ago

Another workaround without recompiling the plugin is to use organized_cloud=true with the plugin, like this:

  <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
  <xacro:VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true" organize_cloud="true">
    <origin xyz="0.025 0 0.175" rpy="0 0 0" />
  </xacro:VLP-16>