test_slide.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, 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 the Willow Garage 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 
35 ## Gazebo collision validation
36 PKG = 'pr2_gazebo'
37 NAME = 'test_slide'
38 
39 import roslib
40 roslib.load_manifest(PKG)
41 
42 import unittest, sys, os, math
43 import time
44 import rospy, rostest
45 from nav_msgs.msg import Odometry
46 
47 TEST_DURATION = 90.0
48 TARGET_X = -6.0 #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
49 TARGET_Y = 0.0 #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
50 TARGET_Z = 3.8
51 TARGET_RAD = 4.5
52 CUP_HEIGHT = 4.0
53 MIN_HITS = 200.0
54 MIN_RUNS = 20.0
55 
56 class TestSlide(unittest.TestCase):
57  def __init__(self, *args):
58  super(TestSlide, self).__init__(*args)
59  self.success = False
60  self.fail = False
61  self.hits = 0
62  self.runs = 0
63  self.print_header = False
64 
65  def positionInput(self, p3d):
66  if not self.print_header:
67  self.print_header = True
68  rospy.loginfo("runs hits x y z dx dy dz dr r_tol")
69 
70  self.runs = self.runs + 1
71  #if (pos.frame == 1):
72  dx = p3d.pose.pose.position.x - TARGET_X
73  dy = p3d.pose.pose.position.y - TARGET_Y
74  dz = p3d.pose.pose.position.z - TARGET_Z
75  d = math.sqrt((dx * dx) + (dy * dy)) #+ (dz * dz))
76 
77  rospy.loginfo("self.runs, self.hits, "\
78  "p3d.pose.pose.position.x , p3d.pose.pose.position.y , p3d.pose.pose.position.z, "\
79  "dx , dy , dz , d, TARGET_RAD")
80 
81  if (d < TARGET_RAD and abs(dz) < CUP_HEIGHT):
82  self.hits = self.hits + 1
83  if (self.runs > 10 and self.runs < 50):
84  rospy.logwarn("Got to goal too quickly! (",self.runs,")")
85  self.success = False
86  self.fail = True
87 
88  if (self.hits > MIN_HITS):
89  if (self.runs > MIN_RUNS):
90  self.success = True
91 
92 
93 
94  def test_slide(self):
95  print "LINK\n"
96  rospy.Subscriber("/base_pose_ground_truth", Odometry, self.positionInput)
97  rospy.init_node(NAME, anonymous=True)
98  timeout_t = time.time() + TEST_DURATION
99  while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
100  time.sleep(0.1)
101  time.sleep(2.0)
102  self.assert_(self.success)
103 
104 
105 
106 
107 if __name__ == '__main__':
108  rostest.run(PKG, sys.argv[0], TestSlide, sys.argv) #, text_mode=True)
109 
110 
def test_slide(self)
Definition: test_slide.py:94
def positionInput(self, p3d)
Definition: test_slide.py:65
def __init__(self, args)
Definition: test_slide.py:57


pr2_gazebo
Author(s): John Hsu
autogenerated on Fri May 3 2019 02:24:27