Merge pull request #192 from at-wat/fix-stl-assertion-on-recent-libstdc++

Fix STL assertion on recent libstdc++ when handling empty PointCloud2

Author: at-wat

sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h

@@ -249,9 +249,9 @@ PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase(C &cloud_msg, c
 {
   int offset = set_field(cloud_msg, field_name);
 
-  data_char_ = &(cloud_msg.data.front()) + offset;
+  data_char_ = cloud_msg.data.data() + offset;
   data_ = reinterpret_cast<TT*>(data_char_);
-  data_end_ = reinterpret_cast<TT*>(&(cloud_msg.data.back()) + 1 + offset);
+  data_end_ = reinterpret_cast<TT*>(data_char_ + cloud_msg.data.size());
 }
 
 /** Assignment operator

sensor_msgs/test/main.cpp

@@ -34,8 +34,24 @@
 
 #include <gtest/gtest.h>
 
+#define _GLIBCXX_DEBUG  // Enable assertions in STL
+
 #include <sensor_msgs/point_cloud2_iterator.h>
 
+TEST(sensor_msgs, PointCloud2Iterator_Empty)
+{
+  EXPECT_EXIT({
+    sensor_msgs::PointCloud2 cloud_msg;
+    cloud_msg.width = 1;
+    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
+    modifier.setPointCloud2FieldsByString(1, "xyz");
+
+    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
+
+    exit(0);
+  }, ::testing::ExitedWithCode(0), "");
+}
+
 TEST(sensor_msgs, PointCloud2Iterator)
 {
   // Create a dummy PointCloud2
@@ -111,6 +127,17 @@ TEST(sensor_msgs, PointCloud2Iterator)
     iter_const_2_g += 1;
     iter_const_2_b = iter_const_2_b + 1;
   }
+  EXPECT_FALSE(iter_const_1_y != iter_const_1_y.end());
+  EXPECT_FALSE(iter_const_1_z != iter_const_1_z.end());
+  EXPECT_FALSE(iter_const_1_r != iter_const_1_r.end());
+  EXPECT_FALSE(iter_const_1_g != iter_const_1_g.end());
+  EXPECT_FALSE(iter_const_1_b != iter_const_1_b.end());
+  EXPECT_FALSE(iter_const_2_x != iter_const_2_x.end());
+  EXPECT_FALSE(iter_const_2_y != iter_const_2_y.end());
+  EXPECT_FALSE(iter_const_2_z != iter_const_2_z.end());
+  EXPECT_FALSE(iter_const_2_r != iter_const_2_r.end());
+  EXPECT_FALSE(iter_const_2_g != iter_const_2_g.end());
+  EXPECT_FALSE(iter_const_2_b != iter_const_2_b.end());
   EXPECT_EQ(i, n_points);
 }