Merge pull request #37 from knorth55/image-publish

publish smach viewer image

smach_viewer/scripts/smach_viewer.py

@@ -35,6 +35,7 @@
 import roslib
 
 from smach_msgs.msg import SmachContainerStatus,SmachContainerInitialStatusCmd,SmachContainerStructure
+from sensor_msgs.msg import Image
 
 import sys
 import os
@@ -48,6 +49,8 @@
     from io import StringIO  # for Python 3
 import colorsys
 import time
+import cv_bridge
+import numpy as np
 import base64
 
 try:
@@ -769,6 +772,8 @@ def __init__(self):
         self._structure_subs = {}
         self._status_subs = {}
 
+        self._pub = rospy.Publisher('~image', Image, queue_size=1)
+
         self.Bind(wx.EVT_IDLE,self.OnIdle)
         self.Bind(wx.EVT_CLOSE,self.OnQuit)
 
@@ -788,6 +793,11 @@ def __init__(self):
         self._update_tree_thread = threading.Thread(target=self._update_tree)
         self._update_tree_thread.start()
 
+        self.timer = wx.Timer(self, 0)
+        self.Bind(wx.EVT_TIMER, self.OnTimer)
+        self.timer.Start(200)
+
+
     def OnQuit(self,event):
         """Quit Event: kill threads and wait for join."""
         with self._update_cond:
@@ -797,7 +807,6 @@ def OnQuit(self,event):
         self._server_list_thread.join()
         self._update_graph_thread.join()
         self._update_tree_thread.join()
-
         event.Skip()
 
     def update_graph(self):
@@ -1180,6 +1189,21 @@ def _update_server_list(self):
             #    self.server_combo.SetStringSelection(self._servers[0])
             #    self.set_server(self._servers[0])
 
+    def OnTimer(self, event):
+        # image
+        context = wx.ClientDC(self)
+        memory = wx.MemoryDC()
+        x, y = self.ClientSize
+        bitmap = wx.EmptyBitmap(x, y, -1)
+        memory.SelectObject(bitmap)
+        memory.Blit(0, 0, x, y, context, 0, 0)
+        memory.SelectObject(wx.NullBitmap)
+        buf = wx.ImageFromBitmap(bitmap).GetDataBuffer()
+        img = np.frombuffer(buf, dtype=np.uint8)
+        bridge = cv_bridge.CvBridge()
+        img_msg = bridge.cv2_to_imgmsg(img.reshape((y, x, 3)), encoding='rgb8')
+        self._pub.publish(img_msg)
+
     def ShowControlsDialog(self,event):
         dial = wx.MessageDialog(None,
                 "Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R",