Source Code for Module rosh.impl.py_image_view

  1  #!/usr/bin/env python 
  2  # Software License Agreement (BSD License) 
  3  # 
  4  # Copyright (c) 2009, Willow Garage, Inc. 
  5  # All rights reserved. 
  6  # 
  7  # Redistribution and use in source and binary forms, with or without 
  8  # modification, are permitted provided that the following conditions 
  9  # are met: 
 10  # 
 11  #  * Redistributions of source code must retain the above copyright 
 12  #    notice, this list of conditions and the following disclaimer. 
 13  #  * Redistributions in binary form must reproduce the above 
 14  #    copyright notice, this list of conditions and the following 
 15  #    disclaimer in the documentation and/or other materials provided 
 16  #    with the distribution. 
 17  #  * Neither the name of Willow Garage, Inc. nor the names of its 
 18  #    contributors may be used to endorse or promote products derived 
 19  #    from this software without specific prior written permission. 
 20  # 
 21  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 22  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 23  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 24  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 25  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 26  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 27  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 28  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 29  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 30  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 31  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 32  # POSSIBILITY OF SUCH DAMAGE. 
 33  # 
 34  # Revision $Id: py_image_view.py 11214 2010-09-23 05:06:22Z kwc $ 
 35   
 36  PKG = 'rosh' 
 37  import roslib; roslib.load_manifest(PKG) 
 38  import rospy 
 39  import optparse 
 40  import sys 
 41  import wx 
 42  import Image 
 43   
 44 -class PyImageViewFrame(wx.Frame): 
 45 -    def __init__(self, topic, map=False): 
 46          if map: 
 47              wx.Frame.__init__(self, None, wx.ID_ANY, topic, pos=wx.DefaultPosition, size=(1000, 1000), style=wx.DEFAULT_FRAME_STYLE) 
 48          else: 
 49              wx.Frame.__init__(self, None, wx.ID_ANY, topic, pos=wx.DefaultPosition, size=(640, 480), style=wx.DEFAULT_FRAME_STYLE) 
 50               
 51          self.image  = None 
 52          self.map  = None 
 53          self.bitmap = None 
 54          self.dirty = False 
 55   
 56          self.Bind(wx.EVT_PAINT, self.on_paint) 
 57          self.Bind(wx.EVT_SIZE,  lambda e: self.convert_bitmap()) 
 58   
 59          if not map: 
 60              import sensor_msgs.msg 
 61              rospy.Subscriber(topic, sensor_msgs.msg.Image, lambda img_msg: wx.CallAfter(self.set_image_msg, img_msg)) 
 62          else: 
 63              import nav_msgs.msg 
 64              rospy.Subscriber(topic, nav_msgs.msg.OccupancyGrid, lambda map_msg: wx.CallAfter(self.set_map_msg, map_msg)) 
 65   
 66 -    def set_image_msg(self, img_msg): 
 67          self.image = self.imgmsg_to_wx(img_msg) 
 68          self.dirty = True 
 69          self.convert_bitmap() 
 70   
 71 -    def set_map_msg(self, msg): 
 72          self.image = self.occgridmsg_to_wx(msg) 
 73          self.dirty = True 
 74          self.convert_bitmap() 
 75           
 76 -    def on_paint(self, event): 
 77          if self.bitmap: 
 78              wx.ClientDC(self).DrawBitmap(self.bitmap, 0, 0) 
 79   
 80 -    def convert_bitmap(self): 
 81          if self.image: 
 82              width = self.image.GetWidth() 
 83              height = self.image.GetHeight() 
 84              target_width, target_height = self.GetClientSize() 
 85   
 86              if self.dirty or ((width, height) != (target_width, target_height)): 
 87                  # Rescale operates in place 
 88                  cp = self.image.Copy() 
 89                  # 1) try to box to width first 
 90                  new_width = target_width 
 91                  new_height = int((float(target_width) / float(width)) * float(height)) 
 92                  if new_height > target_height: 
 93                      # 1) try to box to height instead 
 94                      new_height = target_height 
 95                      new_width = int((float(target_height) / float(height)) * float(width)) 
 96   
 97                  cp.Rescale(new_width, new_height) 
 98                  cp.Resize((self.GetClientSize()[0], self.GetClientSize()[1]), wx.Point(0, 0), 0, 0, 0) 
 99                  self.bitmap = cp.ConvertToBitmap() 
100                  self.dirty = False 
101              self.Refresh() 
102   
103 -    def imgmsg_to_wx(self, img_msg): 
104          if img_msg.encoding == 'rgb8': 
105              return wx.ImageFromBuffer(img_msg.width, img_msg.height, img_msg.data) 
106          elif img_msg.encoding in ['mono8', 'bayer_rggb8', 'bayer_bggr8', 'bayer_gbrg8', 'bayer_grbg8']: 
107              mode = 'L' 
108          elif img_msg.encoding == 'rgb8': 
109              mode = 'RGB' 
110          elif img_msg.encoding == 'bgr8': 
111              mode = 'BGR' 
112          elif img_msg.encoding == 'mono16': 
113              if img_msg.is_bigendian: 
114                  mode = 'F;16B' 
115              else: 
116                  mode = 'F:16' 
117          elif img_msg.encoding in ['rgba8', 'bgra8']: 
118              return None 
119   
120          pil_img = Image.frombuffer('RGB', (img_msg.width, img_msg.height), img_msg.data, 'raw', mode, 0, 1) 
121          if pil_img.mode != 'RGB': 
122              pil_img = pil_img.convert('RGB') 
123   
124          return wx.ImageFromData(pil_img.size[0], pil_img.size[1], pil_img.tostring()) 
125   
126 -    def occgridmsg_to_wx(self, map): 
127          # adapted from map_tiler 
128          size = (map.info.width, map.info.height) 
129          s = "".join([maptrans(x) for x in map.data]) 
130          return wx.ImageFromData(map.info.width, map.info.height, s) 
131   
132  s128 = chr(128)*3 
133  s0 = chr(0)*3 
134  s255 = chr(255)*3 
135 -def maptrans(x): 
136    if x == -1: 
137        return s128 
138    elif x == 0: 
139        return s255 
140    elif x == 100: 
141        return s0 
142    else: 
143        return chr(x)*3 
144                           
145  if __name__ == '__main__': 
146      parser = optparse.OptionParser() 
147      parser.add_option('-t', '--topic', action='store', default='/image', help='topic to listen to image msg on') 
148      parser.add_option('-m', '--map', action='store_true', help='topic is is nav_msgs/OccupancyGrid') 
149      options, args = parser.parse_args(sys.argv[1:]) 
150   
151      rospy.init_node('py_image_view', anonymous=True) 
152   
153      app = wx.PySimpleApp() 
154      frame = PyImageViewFrame(options.topic, map=options.map) 
155      frame.Show() 
156      app.MainLoop() 
157