test_slide.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 collision validation 
00036 PKG = 'pr2_gazebo'
00037 NAME = 'test_slide'
00038 
00039 import roslib
00040 roslib.load_manifest(PKG)
00041 
00042 import unittest, sys, os, math
00043 import time
00044 import rospy, rostest
00045 from nav_msgs.msg import Odometry
00046 
00047 TEST_DURATION = 90.0
00048 TARGET_X = -6.0  #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
00049 TARGET_Y = 0.0  #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
00050 TARGET_Z = 3.8
00051 TARGET_RAD = 4.5
00052 CUP_HEIGHT = 4.0
00053 MIN_HITS = 200.0
00054 MIN_RUNS = 20.0
00055 
00056 class TestSlide(unittest.TestCase):
00057     def __init__(self, *args):
00058         super(TestSlide, self).__init__(*args)
00059         self.success = False
00060         self.fail = False
00061         self.hits = 0
00062         self.runs = 0
00063         self.print_header = False
00064         
00065     def positionInput(self, p3d):
00066         if not self.print_header: 
00067           self.print_header = True
00068           print "runs   hits   x   y   z    dx    dy    dz    dr   r_tol"
00069 
00070         self.runs = self.runs + 1
00071         #if (pos.frame == 1):
00072         dx = p3d.pose.pose.position.x - TARGET_X
00073         dy = p3d.pose.pose.position.y - TARGET_Y
00074         dz = p3d.pose.pose.position.z - TARGET_Z
00075         d = math.sqrt((dx * dx) + (dy * dy)) #+ (dz * dz))
00076 
00077         print self.runs, self.hits, \
00078               p3d.pose.pose.position.x , p3d.pose.pose.position.y , p3d.pose.pose.position.z, \
00079               dx , dy , dz , d, TARGET_RAD
00080 
00081         if (d < TARGET_RAD and abs(dz) < CUP_HEIGHT):
00082             self.hits = self.hits + 1
00083             if (self.runs > 10 and self.runs < 50):
00084                 print "Got to goal too quickly! (",self.runs,")"
00085                 self.success = False
00086                 self.fail = True
00087 
00088             if (self.hits > MIN_HITS):
00089                 if (self.runs > MIN_RUNS):
00090                     self.success = True
00091 
00092         
00093     
00094     def test_slide(self):
00095         print "LINK\n"
00096         rospy.Subscriber("/base_pose_ground_truth", Odometry, self.positionInput)
00097         rospy.init_node(NAME, anonymous=True)
00098         timeout_t = time.time() + TEST_DURATION
00099         while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
00100             time.sleep(0.1)
00101         time.sleep(2.0)
00102         self.assert_(self.success)
00103         
00104     
00105 
00106 
00107 if __name__ == '__main__':
00108     rostest.run(PKG, sys.argv[0], TestSlide, sys.argv) #, text_mode=True)
00109 
00110 


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