From 60cb1835db88d58b2321a0e23af09664e8a71de9 Mon Sep 17 00:00:00 2001
From: axelschroth <axel.schroth@gmail.com>
Date: Sat, 29 Apr 2017 03:36:25 +0200
Subject: [PATCH] Update display if empty pointcloud2 is published (#1073)

Do not show last point cloud any more, if published point cloud message does not contain any points
---
 .../default_plugin/point_cloud2_display.cpp   | 65 ++++++++++---------
 1 file changed, 33 insertions(+), 32 deletions(-)

diff --git a/src/rviz/default_plugin/point_cloud2_display.cpp b/src/rviz/default_plugin/point_cloud2_display.cpp
index f4990f897b..19c4d390fd 100644
--- a/src/rviz/default_plugin/point_cloud2_display.cpp
+++ b/src/rviz/default_plugin/point_cloud2_display.cpp
@@ -106,50 +106,51 @@ void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr&
   }
 
   filtered->data.resize(cloud->data.size());
+  uint32_t output_count;
   if (point_count == 0)
   {
-    return;
-  }
-
-  uint8_t* output_ptr = &filtered->data.front();
-  const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
-  size_t points_to_copy = 0;
-  for (; ptr < ptr_end; ptr += point_step)
-  {
-    float x = *reinterpret_cast<const float*>(ptr + xoff);
-    float y = *reinterpret_cast<const float*>(ptr + yoff);
-    float z = *reinterpret_cast<const float*>(ptr + zoff);
-    if (validateFloats(x) && validateFloats(y) && validateFloats(z))
+    output_count = 0;
+  } else {
+    uint8_t* output_ptr = &filtered->data.front();
+    const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
+    size_t points_to_copy = 0;
+    for (; ptr < ptr_end; ptr += point_step)
     {
-      if (points_to_copy == 0)
+      float x = *reinterpret_cast<const float*>(ptr + xoff);
+      float y = *reinterpret_cast<const float*>(ptr + yoff);
+      float z = *reinterpret_cast<const float*>(ptr + zoff);
+      if (validateFloats(x) && validateFloats(y) && validateFloats(z))
       {
-        // Only memorize where to start copying from
-        ptr_init = ptr;
-        points_to_copy = 1;
+        if (points_to_copy == 0)
+        {
+          // Only memorize where to start copying from
+          ptr_init = ptr;
+          points_to_copy = 1;
+        }
+        else
+        {
+          ++points_to_copy;
+        }
       }
       else
       {
-        ++points_to_copy;
+        if (points_to_copy)
+        {
+          // Copy all the points that need to be copied
+          memcpy(output_ptr, ptr_init, point_step*points_to_copy);
+          output_ptr += point_step*points_to_copy;
+          points_to_copy = 0;
+        }
       }
     }
-    else
+    // Don't forget to flush what needs to be copied
+    if (points_to_copy)
     {
-      if (points_to_copy)
-      {
-        // Copy all the points that need to be copied
-        memcpy(output_ptr, ptr_init, point_step*points_to_copy);
-        output_ptr += point_step*points_to_copy;
-        points_to_copy = 0;
-      }
+      memcpy(output_ptr, ptr_init, point_step*points_to_copy);
+      output_ptr += point_step*points_to_copy;
     }
+    output_count = (output_ptr - &filtered->data.front()) / point_step;
   }
-  // Don't forget to flush what needs to be copied
-  if (points_to_copy)
-  {
-    memcpy(output_ptr, ptr_init, point_step*points_to_copy);
-    output_ptr += point_step*points_to_copy;
-  }
-  uint32_t output_count = (output_ptr - &filtered->data.front()) / point_step;
 
   filtered->header = cloud->header;
   filtered->fields = cloud->fields;