test_link_pose.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 test utility
36 ## check a link's pose against some reference pose
37 ## good for unit testing proper behavior
38 
39 PKG = 'gazebo_plugins'
40 NAME = 'test_link_pose'
41 
42 import math
43 import roslib
44 roslib.load_manifest(PKG)
45 
46 import sys, unittest
47 import os, os.path, threading, time
48 import rospy, rostest
49 from nav_msgs.msg import *
50 from geometry_msgs.msg import Pose
51 from geometry_msgs.msg import Twist
52 from tf import transformations
53 
54 class LinkPoseTest(unittest.TestCase):
55  def __init__(self, *args):
56  super(LinkPoseTest, self).__init__(*args)
57  self.success = False
58 
60  self.link_error_rms = 0
63  self.link_error_max = 0
64 
65  self.min_link_samples_topic = "~min_link_samples"
66  self.min_link_samples = 1000
67 
68  self.min_valid_samples_topic = "~min_valid_samples"
70 
71  self.tolerance_topic = "~error_tolerance"
72  self.tolerance = 0.01
73 
74  self.max_error_topic = "~max_error"
75  self.max_error = 0.02
76 
77  # in seconds
78  self.test_duration_topic = "~test_duration"
79  self.test_duration = 10.0
80 
81  # test start time in seconds
82  self.test_start_time_topic = "~test_start_time"
83  self.test_start_time = 0.0
84 
85  self.link_pose_topic_name = "~link_pose_topic_name"
86  self.link_pose_topic = "/model_1/link_2/pose"
87 
88  self.valid_pose_topic_name = "~valid_pose_topic_name"
89  self.valid_pose_topic = "/p3d_valid"
90 
91  self.link_pose = Pose()
92  self.valid_pose = Pose()
93 
94  def printLinkPose(self, p3d):
95  print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x)
96  print " " + "y: " + str(p3d.pose.pose.position.y)
97  print " " + "z: " + str(p3d.pose.pose.position.z)
98  print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x)
99  print " " + "y: " + str(p3d.pose.pose.orientation.y)
100  print " " + "z: " + str(p3d.pose.pose.orientation.z)
101  print " " + "w: " + str(p3d.pose.pose.orientation.w)
102  print "P3D rate translan: " + "x: " + str(p3d.twist.twist.linear.x)
103  print " " + "y: " + str(p3d.twist.twist.linear.y)
104  print " " + "z: " + str(p3d.twist.twist.linear.z)
105  print "P3D rate rotation: " + "x: " + str(p3d.twist.twist.angular.x)
106  print " " + "y: " + str(p3d.twist.twist.angular.y)
107  print " " + "z: " + str(p3d.twist.twist.angular.z)
108 
109 
110  def linkP3dInput(self, p3d):
111  #self.printLinkPose(p3d)
112  self.link_pose = p3d.pose.pose
113 
114  # start logging after we get minimum valid pose specified
115  # todo: synchronize the two input streams somehow
116  if self.valid_sample_count >= self.min_valid_samples:
117  self.link_sample_count += 1
118  tmpx = self.link_pose.position.x - self.valid_pose.position.x
119  tmpy = self.link_pose.position.y - self.valid_pose.position.y
120  tmpz = self.link_pose.position.z - self.valid_pose.position.z
121  error = math.sqrt(tmpx*tmpx+tmpy*tmpy+tmpz*tmpz)
122  self.link_error_total += error
124  if error > self.link_error_max:
125  self.link_error_max = math.sqrt(tmpx*tmpx+tmpy*tmpy+tmpz*tmpz)
126 
127  def validP3dInput(self, p3d):
128  self.valid_pose = p3d.pose.pose
129  self.valid_sample_count += 1
130 
131  def checkPose(self):
132  # difference in pose
133  print "error: " + str(self.link_sample_count) \
134  + " error:" + str(self.link_error_total) \
135  + " avg err:" + str(self.link_error_rms) \
136  + " max err:" + str(self.link_error_max) \
137  + " count: " + str(self.link_sample_count)
138  if self.link_sample_count >= self.min_link_samples:
139  if self.link_error_rms < self.tolerance:
140  if self.link_error_max < self.max_error:
141  self.success = True
142 
143 
144  def test_link_pose(self):
145  print "LNK\n"
146  rospy.init_node(NAME, anonymous=True)
147  self.link_pose_topic = rospy.get_param(self.link_pose_topic_name,self.link_pose_topic);
148  self.valid_pose_topic = rospy.get_param(self.valid_pose_topic_name,self.valid_pose_topic);
149  self.min_link_samples = rospy.get_param(self.min_link_samples_topic,self.min_link_samples);
150  self.min_valid_samples = rospy.get_param(self.min_valid_samples_topic,self.min_valid_samples);
151  self.tolerance = rospy.get_param(self.tolerance_topic,self.tolerance);
152  self.max_error = rospy.get_param(self.max_error_topic,self.max_error);
153  self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration);
154  self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time);
155 
156  while not rospy.is_shutdown() and time.time() < self.test_start_time:
157  rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time)
158  time.sleep(0.1)
159 
160  print "subscribe"
161  rospy.Subscriber(self.link_pose_topic, Odometry, self.linkP3dInput)
162  rospy.Subscriber(self.valid_pose_topic, Odometry, self.validP3dInput)
163 
164  start_t = time.time()
165  timeout_t = start_t + self.test_duration
166  while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
167  self.checkPose()
168  time.sleep(0.1)
169  self.assert_(self.success)
170 
171 if __name__ == '__main__':
172  print "Waiting for test to start at time "
173  rostest.run(PKG, sys.argv[0], LinkPoseTest, sys.argv) #, text_mode=True)
174 
175 


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27