testSingleShot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # Software License Agreement (Modified BSD License)
00005 #
00006 #  Copyright (c) 2012, PAL Robotics, S.L.
00007 #  All rights reserved.
00008 #
00009 #  Redistribution and use in source and binary forms, with or without
00010 #  modification, are permitted provided that the following conditions
00011 #  are met:
00012 #
00013 #   * Redistributions of source code must retain the above copyright
00014 #     notice, this list of conditions and the following disclaimer.
00015 #   * Redistributions in binary form must reproduce the above
00016 #     copyright notice, this list of conditions and the following
00017 #     disclaimer in the documentation and/or other materials provided
00018 #     with the distribution.
00019 #   * Neither the name of PAL Robotics, S.L. nor the names of its
00020 #     contributors may be used to endorse or promote products derived
00021 #     from this software without specific prior written permission.
00022 #
00023 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #  POSSIBILITY OF SUCH DAMAGE.
00035 #
00036 
00037 import roslib; roslib.load_manifest('pal_blort')
00038 import re
00039 import subprocess
00040 from multiprocessing import Process
00041 from geometry_msgs.msg import Pose
00042 import sys
00043 from call_SingleShot import singleShotClient
00044 from pose_utils import *
00045 
00046 # error in meters
00047 max_error = Pose()
00048 max_error.position.x = 0.3
00049 max_error.position.y = 0.3
00050 max_error.position.z = 0.3
00051 max_error.orientation.x = 0.2
00052 max_error.orientation.y = 0.2
00053 max_error.orientation.z = 0.2
00054 max_error.orientation.w = 0.2
00055 
00056 if __name__ == "__main__":
00057     if len(sys.argv) < 3:
00058         print "First the path of the pose file to compare to, second parameter should be the number of tries!"
00059         exit(1)
00060     approxPose = readPose(sys.argv[1])
00061     print "'ve read approxPose : "
00062     print approxPose
00063 
00064     nTries = int(sys.argv[2])
00065     nFails = 0
00066     nSuccs = 0
00067     resultPoses = []
00068 
00069     for i in range(nTries):
00070         resultPose = singleShotClient()
00071         if resultPose == None:
00072             nFails += 1
00073         else:
00074             if poseValidate(approxPose, resultPose, max_error):
00075                  nSuccs += 1
00076                  resultPoses.append(resultPose)
00077             else:
00078                 nFails += 1
00079     print '\nStatistics: nTries: '+ repr(nTries) + ', nFails: ' + repr(nFails) + ', nSuccs: ' + repr(nSuccs)
00080     if nSuccs > 0:
00081         print 'Average error:'
00082         print poseListAvgDiff(approxPose, resultPoses)
00083 
00084     print "\nBig Kahuna Burger. That's that Hawaiian burger joint."
00085 
00086 


blort_ros
Author(s): Bence Magyar
autogenerated on Thu Jan 2 2014 11:39:13