test_camera.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, 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 the Willow Garage 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 
00035 ## Gazebo test cameras validation 
00036 
00037 PKG = 'pr2_gazebo'
00038 NAME = 'test_camera'
00039 
00040 import math
00041 import roslib
00042 roslib.load_manifest(PKG)
00043 roslib.load_manifest('rostest')
00044 roslib.load_manifest('actionlib')
00045 roslib.load_manifest('pr2_controllers_msgs')
00046 
00047 import sys, unittest
00048 import os, os.path, threading, time
00049 import rospy, rostest, string
00050 from std_msgs.msg import Int32
00051 from sensor_msgs.msg import Image as image_msg
00052 from sensor_msgs.msg import CameraInfo as camerainfo_msg
00053 from PIL import Image      as pili
00054 from PIL import ImageChops as pilic
00055 from geometry_msgs.msg import PointStamped, Point, Vector3
00056 from pr2_controllers_msgs.msg import PointHeadGoal, PointHeadAction
00057 import actionlib.action_client
00058 
00059 #FRAME_TARGET = "cam_sen-0050.ppm"
00060 FRAME_TARGET = "test_camera.valid.ppm"
00061 FRAME_DIR = "test_camera_frames"
00062 TOTAL_ERROR_TOL = 5
00063 TEST_DURATION   = 70
00064 
00065 class PollCameraThread(threading.Thread):
00066     def __init__(self, target, dir):
00067         super(PollCameraThread, self).__init__()
00068         self.target = target
00069         self.dir = dir
00070     
00071     def run(self):
00072         while 1:
00073             time.sleep(0.01)
00074             if (os.path.isdir(self.dir)):
00075                 ls = os.listdir(self.dir)
00076                 #print "Dir: " + str(ls)
00077                 for file in ls:
00078                     if (file == FRAME_TARGET):
00079                         self.target.onTargetFrame()
00080                         return
00081 
00082 class TestCameras(unittest.TestCase):
00083     def __init__(self, *args):
00084         super(TestCameras, self).__init__(*args)
00085         self.success = False
00086         self.got_camerainfo = False
00087         self.camerainfo_height = 0
00088         self.camerainfo_width = 0
00089 
00090     def onTargetFrame(self):
00091         time.sleep(0.5) #Safety, to make sure the image is really done being written.
00092         ps = "diff -q " + FRAME_DIR + "/" + FRAME_TARGET + " test/test_camera.valid.ppm"
00093         #print "CMD: " + ps + "\n"
00094         result = os.system(ps)
00095         if (result == 0):
00096             self.success = True
00097             #print "Success\n"
00098         else:
00099             self.success = False
00100             #print "Failure\n"
00101         rospy.signal_shutdown('test done')
00102 
00103     def images_are_the_same(self,i0,i1):
00104         print "comparing images... "
00105         error_count = 0
00106         error_total = 0
00107         pixel_tol = 0
00108 
00109         # thumbnail-ize images
00110         size = 100,100
00111         i0.thumbnail(size,pili.ANTIALIAS)
00112         i1.thumbnail(size,pili.ANTIALIAS)
00113         ic = pilic.difference(i0,i1)
00114         i0.save("t_0.ppm")
00115         i1.save("t_1.ppm")
00116         ic.save("t_d.ppm")
00117 
00118         # get raw data for comparison
00119         i0d = i0.getdata()
00120         i1d = i1.getdata()
00121 
00122         # assert len(i0)==len(i1)
00123         if len(i0d) != len(i1d):
00124           print "lengths not equal ",len(i0d), len(i1d)
00125           return False
00126         print "thumbnail lengths ",len(i0d), len(i1d)
00127 
00128         #compare pixel by pixel for thumbnails
00129         for i in range(len(i0d)):
00130           (p0) = i0d[i]
00131           (p1) = i1d[i]
00132           if abs(p0-p1) > pixel_tol:
00133             error_count = error_count + 1
00134             error_total = error_total + abs(p0-p1)
00135         error_avg = float(error_total)/float(len(i0d))
00136         print "total error count:",error_count
00137         print "average error:    ",error_avg
00138         if error_avg > TOTAL_ERROR_TOL:
00139           return False
00140         else:
00141           return True
00142 
00143     def camerainfoInput(self,camerainfo):
00144       self.got_camerainfo = True
00145       self.camerainfo_width = camerainfo.width
00146       self.camerainfo_height = camerainfo.height
00147       print " got cam info (",camerainfo.width,"X",camerainfo.height,")"
00148 
00149     def imageInput(self,image):
00150 
00151       if (self.got_camerainfo):
00152         print " got image and camerainfo from ROS, begin comparing images."
00153       else:
00154         print " got image but no camerainfo."
00155         return
00156 
00157       print "  - load validation image from file test_camera.valid.ppm "
00158       fname = roslib.packages.get_pkg_dir('pr2_gazebo') + '/test/sensors/test_camera.valid.ppm'
00159       if os.path.isfile(fname):
00160         im0 = pili.open(fname)
00161       else:
00162         print "cannot find validation file: test_camera.valid.ppm"
00163         self.success = False
00164         return
00165 
00166       print "  - load image from ROS "
00167       size = self.camerainfo_width,self.camerainfo_height
00168       im1 = pili.new("L",size)
00169       im1 = pili.frombuffer("L",size,str(image.data));
00170       im1 = im1.transpose(pili.FLIP_LEFT_RIGHT).rotate(180);
00171       imc = pilic.difference(im0,im1)
00172 
00173 
00174       print "  - comparing images "
00175       im1.save("test_1.ppm") # uncomment this line to capture a new valid frame when things change
00176       im0.save("test_0.ppm")
00177       imc.save("test_d.ppm")
00178       #im1.show()
00179       #im0.show()
00180       #imc.show()
00181       comp_result = self.images_are_the_same(im0,im1)
00182       print "test comparison ", comp_result
00183       #print "proofcomparison ", self.images_are_the_same(im1.getdata(),im1.getdata())
00184       if (self.success == False): # test if we have not succeeded
00185         if (comp_result == True):
00186           print "  - images are the Same "
00187           self.success = True
00188         else:
00189           print "  - images differ "
00190           self.success = False
00191 
00192 
00193     def test_camera(self):
00194         rospy.init_node(NAME, anonymous=True)
00195         head_action_client = actionlib.action_client.ActionClient('/head_traj_controller/point_head_action',PointHeadAction)
00196         def send_head_goal(g):
00197             g.target.header.stamp = rospy.get_rostime()
00198             return head_action_client.send_goal(g)
00199         head_goal = PointHeadGoal()
00200         head_goal.target.header.frame_id = 'base_link'
00201         head_goal.target.point = Point(10,0,0.55)
00202 
00203         projector_pub = rospy.Publisher("/projector_wg6802418_controller/projector", Int32, latch=True);
00204         projector_pub.publish(Int32(0));
00205 
00206         print " subscribe stereo left image from ROS "
00207         rospy.Subscriber("/narrow_stereo/left/image_raw", image_msg, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera)
00208         rospy.Subscriber("/narrow_stereo/left/camera_info", camerainfo_msg, self.camerainfoInput) # this is a camera mounted on PR2 head (left stereo camera)
00209         timeout_t = time.time() + TEST_DURATION
00210         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
00211             head_goal_status = send_head_goal(head_goal)
00212             time.sleep(1.0)
00213         self.assert_(self.success)
00214         
00215     
00216 
00217 
00218 if __name__ == '__main__':
00219     #while (os.path.isfile(FRAME_DIR + "/" + FRAME_TARGET)):
00220     #    print "Old test case still alive."
00221     #    time.sleep(1)
00222     
00223     print " starting test "
00224     rostest.run(PKG, sys.argv[0], TestCameras, sys.argv)
00225 
00226 


pr2_gazebo
Author(s): John Hsu
autogenerated on Sat Sep 5 2015 11:23:01