py_image_view.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # Revision $Id: py_image_view.py 11141 2010-09-18 00:25:37Z kwc $
00035 
00036 PKG = 'rosh'
00037 import roslib; roslib.load_manifest(PKG)
00038 import rospy
00039 import optparse
00040 import sys
00041 import wx
00042 import Image
00043 
00044 class PyImageViewFrame(wx.Frame):
00045     def __init__(self, topic, map=False):
00046         if map:
00047             wx.Frame.__init__(self, None, wx.ID_ANY, topic, pos=wx.DefaultPosition, size=(1000, 1000), style=wx.DEFAULT_FRAME_STYLE)
00048         else:
00049             wx.Frame.__init__(self, None, wx.ID_ANY, topic, pos=wx.DefaultPosition, size=(640, 480), style=wx.DEFAULT_FRAME_STYLE)
00050             
00051         self.image  = None
00052         self.map  = None
00053         self.bitmap = None
00054         self.dirty = False
00055 
00056         self.Bind(wx.EVT_PAINT, self.on_paint)
00057         self.Bind(wx.EVT_SIZE,  lambda e: self.convert_bitmap())
00058 
00059         if not map:
00060             import sensor_msgs.msg
00061             rospy.Subscriber(topic, sensor_msgs.msg.Image, lambda img_msg: wx.CallAfter(self.set_image_msg, img_msg))
00062         else:
00063             import nav_msgs.msg
00064             rospy.Subscriber(topic, nav_msgs.msg.OccupancyGrid, lambda map_msg: wx.CallAfter(self.set_map_msg, map_msg))
00065 
00066     def set_image_msg(self, img_msg):
00067         self.image = self.imgmsg_to_wx(img_msg)
00068         self.dirty = True
00069         self.convert_bitmap()
00070 
00071     def set_map_msg(self, msg):
00072         self.image = self.occgridmsg_to_wx(msg)
00073         self.dirty = True
00074         self.convert_bitmap()
00075         
00076     def on_paint(self, event):
00077         if self.bitmap:
00078             wx.ClientDC(self).DrawBitmap(self.bitmap, 0, 0)
00079 
00080     def convert_bitmap(self):
00081         if self.image:
00082             width = self.image.GetWidth()
00083             height = self.image.GetHeight()
00084             target_width, target_height = self.GetClientSize()
00085 
00086             if self.dirty or ((width, height) != (target_width, target_height)):
00087                 # Rescale operates in place
00088                 cp = self.image.Copy()
00089                 # 1) try to box to width first
00090                 new_width = target_width
00091                 new_height = int((float(target_width) / float(width)) * float(height))
00092                 if new_height > target_height:
00093                     # 1) try to box to height instead
00094                     new_height = target_height
00095                     new_width = int((float(target_height) / float(height)) * float(width))
00096 
00097                 cp.Rescale(new_width, new_height)
00098                 cp.Resize((self.GetClientSize()[0], self.GetClientSize()[1]), wx.Point(0, 0), 0, 0, 0)
00099                 self.bitmap = cp.ConvertToBitmap()
00100                 self.dirty = False
00101             self.Refresh()
00102 
00103     def imgmsg_to_wx(self, img_msg):
00104         if img_msg.encoding == 'rgb8':
00105             return wx.ImageFromBuffer(img_msg.width, img_msg.height, img_msg.data)
00106         elif img_msg.encoding in ['mono8', 'bayer_rggb8', 'bayer_bggr8', 'bayer_gbrg8', 'bayer_grbg8']:
00107             mode = 'L'
00108         elif img_msg.encoding == 'rgb8':
00109             mode = 'RGB'
00110         elif img_msg.encoding == 'bgr8':
00111             mode = 'BGR'
00112         elif img_msg.encoding == 'mono16':
00113             if img_msg.is_bigendian:
00114                 mode = 'F;16B'
00115             else:
00116                 mode = 'F:16'
00117         elif img_msg.encoding in ['rgba8', 'bgra8']:
00118             return None
00119 
00120         pil_img = Image.frombuffer('RGB', (img_msg.width, img_msg.height), img_msg.data, 'raw', mode, 0, 1)
00121         if pil_img.mode != 'RGB':
00122             pil_img = pil_img.convert('RGB')
00123 
00124         return wx.ImageFromData(pil_img.size[0], pil_img.size[1], pil_img.tostring())
00125 
00126     def occgridmsg_to_wx(self, map):
00127         # adapted from map_tiler
00128         size = (map.info.width, map.info.height)
00129         s = "".join([maptrans(x) for x in map.data])
00130         return wx.ImageFromData(map.info.width, map.info.height, s)
00131 
00132 s128 = chr(128)*3
00133 s0 = chr(0)*3
00134 s255 = chr(255)*3
00135 def maptrans(x):
00136   if x == -1:
00137       return s128
00138   elif x == 0:
00139       return s255
00140   elif x == 100:
00141       return s0
00142   else:
00143       return chr(x)*3
00144                         
00145 if __name__ == '__main__':
00146     parser = optparse.OptionParser()
00147     parser.add_option('-t', '--topic', action='store', default='/image', help='topic to listen to image msg on')
00148     parser.add_option('-m', '--map', action='store_true', help='topic is is nav_msgs/OccupancyGrid')
00149     options, args = parser.parse_args(sys.argv[1:])
00150 
00151     rospy.init_node('py_image_view', anonymous=True)
00152 
00153     app = wx.PySimpleApp()
00154     frame = PyImageViewFrame(options.topic, map=options.map)
00155     frame.Show()
00156     app.MainLoop()


rosh_common
Author(s): Ken Conley
autogenerated on Fri Aug 28 2015 12:40:17