Fix markers flickering in RViz

Fixes NVlabs#119
kini5gowda · May 18, 2020 · 33d771e · 33d771e
1 parent 685eaf9
commit 33d771e
Showing 1 changed file with 10 additions and 7 deletions.
diff --git a/nodes/dope b/nodes/dope
@@ -105,6 +105,7 @@ class DopeNode(object):
         self.meshes = {}
         self.mesh_scales = {}
         self.cv_bridge = CvBridge()
+        self.prev_num_detections = 0
 
         self.input_is_rectified = rospy.get_param('~input_is_rectified', True)
         self.downscale_height = rospy.get_param('~downscale_height', 500)
@@ -310,13 +311,6 @@ class DopeNode(object):
         self.publish_markers(detection_array)
 
     def publish_markers(self, detection_array):
-        # Delete all existing markers
-        markers = MarkerArray()
-        marker = Marker()
-        marker.action = Marker.DELETEALL
-        markers.markers.append(marker)
-        self.pub_markers.publish(markers)
-
         # Object markers
         class_id_to_name = {class_id: name for name, class_id in self.class_ids.iteritems()}
         markers = MarkerArray()
@@ -379,6 +373,15 @@ class DopeNode(object):
                 # user didn't specify self.meshes[name], so don't publish marker
                 pass
 
+        for i in range(len(detection_array.detections), self.prev_num_detections):
+            for ns in ["bboxes", "texts", "meshes"]:
+                marker = Marker()
+                marker.action = Marker.DELETE
+                marker.ns = ns
+                marker.id = i
+                markers.markers.append(marker)
+        self.prev_num_detections = len(detection_array.detections)
+
         self.pub_markers.publish(markers)