Publish mesh markers

kini5gowda · Jun 19, 2019 · ac61792 · ac61792
1 parent 087b28c
commit ac61792
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
diff --git a/config/config_pose.yaml b/config/config_pose.yaml
@@ -47,6 +47,12 @@ draw_colors: {
 
 }
 
+# optional: if you provide a mesh of the object here, a mesh marker will be
+# published for visualization in RViz
+meshes: {
+#  "cracker": "package://dope/meshes/003_cracker_box/google_16k/textured.obj",
+}
+
 # Config params for DOPE
 thresh_angle: 0.5
 thresh_map: 0.01

diff --git a/nodes/dope b/nodes/dope
@@ -101,6 +101,7 @@ class DopeNode(object):
         self.draw_colors = {}
         self.dimensions = {}
         self.class_ids = {}
+        self.meshes = {}
         self.cv_bridge = CvBridge()
 
         self.input_is_rectified = params['input_is_rectified']
@@ -127,6 +128,11 @@ class DopeNode(object):
                 )
             self.models[model].load_net_model()
 
+            try:
+                self.meshes[model] = params['meshes'][model]
+            except KeyError:
+                pass
+
             self.draw_colors[model] = \
                 tuple(params["draw_colors"][model])
             self.dimensions[model] = \
@@ -331,6 +337,28 @@ class DopeNode(object):
             marker.text = '{} ({:.2f})'.format(name, det.results[0].score)
             markers.markers.append(marker)
 
+            # mesh marker
+            try:
+                marker = Marker()
+                marker.header = detection_array.header
+                marker.action = Marker.ADD
+                marker.pose = det.bbox.center
+                marker.color.r = color[0] / 255.0
+                marker.color.g = color[1] / 255.0
+                marker.color.b = color[2] / 255.0
+                marker.color.a = 0.7
+                marker.ns = "meshes"
+                marker.id = i
+                marker.type = Marker.MESH_RESOURCE
+                marker.scale.x = 1.0
+                marker.scale.y = 1.0
+                marker.scale.z = 1.0
+                marker.mesh_resource = self.meshes[name]
+                markers.markers.append(marker)
+            except KeyError:
+                # user didn't specify self.meshes[name], so don't publish marker
+                pass
+
         self.pub_markers.publish(markers)