test_link_pose.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 utility
00036 ##        check a link's pose against some reference pose
00037 ##        good for unit testing proper behavior
00038 
00039 PKG = 'gazebo_plugins'
00040 NAME = 'test_link_pose'
00041 
00042 import math
00043 import roslib
00044 roslib.load_manifest(PKG)
00045 
00046 import sys, unittest
00047 import os, os.path, threading, time
00048 import rospy, rostest
00049 from nav_msgs.msg import *
00050 from geometry_msgs.msg import Pose
00051 from geometry_msgs.msg import Twist
00052 from tf import transformations
00053 
00054 class LinkPoseTest(unittest.TestCase):
00055     def __init__(self, *args):
00056         super(LinkPoseTest, self).__init__(*args)
00057         self.success = False
00058 
00059         self.link_error_total = 0
00060         self.link_error_rms = 0
00061         self.link_sample_count = 0
00062         self.valid_sample_count = 0
00063         self.link_error_max = 0
00064 
00065         self.min_link_samples_topic     = "~min_link_samples"
00066         self.min_link_samples     = 1000
00067 
00068         self.min_valid_samples_topic     = "~min_valid_samples"
00069         self.min_valid_samples     = 0
00070 
00071         self.tolerance_topic = "~error_tolerance"
00072         self.tolerance = 0.01
00073 
00074         self.max_error_topic = "~max_error"
00075         self.max_error = 0.02
00076 
00077         # in seconds
00078         self.test_duration_topic = "~test_duration"
00079         self.test_duration = 10.0
00080 
00081         # test start time in seconds
00082         self.test_start_time_topic = "~test_start_time"
00083         self.test_start_time = 0.0
00084 
00085         self.link_pose_topic_name = "~link_pose_topic_name"
00086         self.link_pose_topic = "/model_1/link_2/pose"
00087 
00088         self.valid_pose_topic_name = "~valid_pose_topic_name"
00089         self.valid_pose_topic = "/p3d_valid"
00090 
00091         self.link_pose = Pose()
00092         self.valid_pose = Pose()
00093 
00094     def printLinkPose(self, p3d):
00095         print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x)
00096         print "                   " + "y: " + str(p3d.pose.pose.position.y)
00097         print "                   " + "z: " + str(p3d.pose.pose.position.z)
00098         print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x)
00099         print "                   " + "y: " + str(p3d.pose.pose.orientation.y)
00100         print "                   " + "z: " + str(p3d.pose.pose.orientation.z)
00101         print "                   " + "w: " + str(p3d.pose.pose.orientation.w)
00102         print "P3D rate translan: " + "x: " + str(p3d.twist.twist.linear.x)
00103         print "                   " + "y: " + str(p3d.twist.twist.linear.y)
00104         print "                   " + "z: " + str(p3d.twist.twist.linear.z)
00105         print "P3D rate rotation: " + "x: " + str(p3d.twist.twist.angular.x)
00106         print "                   " + "y: " + str(p3d.twist.twist.angular.y)
00107         print "                   " + "z: " + str(p3d.twist.twist.angular.z)
00108 
00109 
00110     def linkP3dInput(self, p3d):
00111         #self.printLinkPose(p3d)
00112         self.link_pose = p3d.pose.pose
00113 
00114         # start logging after we get minimum valid pose specified
00115         # todo: synchronize the two input streams somehow
00116         if self.valid_sample_count >= self.min_valid_samples:
00117           self.link_sample_count += 1
00118           tmpx = self.link_pose.position.x - self.valid_pose.position.x
00119           tmpy = self.link_pose.position.y - self.valid_pose.position.y
00120           tmpz = self.link_pose.position.z - self.valid_pose.position.z
00121           error = math.sqrt(tmpx*tmpx+tmpy*tmpy+tmpz*tmpz)
00122           self.link_error_total += error
00123           self.link_error_rms = self.link_error_total / self.link_sample_count
00124           if error > self.link_error_max:
00125               self.link_error_max =  math.sqrt(tmpx*tmpx+tmpy*tmpy+tmpz*tmpz)
00126 
00127     def validP3dInput(self, p3d):
00128         self.valid_pose = p3d.pose.pose
00129         self.valid_sample_count += 1
00130 
00131     def checkPose(self):
00132         # difference in pose
00133         print  "error: "   + str(self.link_sample_count) \
00134                            + " error:" + str(self.link_error_total) \
00135                            + " avg err:" + str(self.link_error_rms) \
00136                            + " max err:" + str(self.link_error_max) \
00137                            + " count: " + str(self.link_sample_count)
00138         if self.link_sample_count >= self.min_link_samples:
00139           if self.link_error_rms < self.tolerance:
00140             if self.link_error_max < self.max_error:
00141               self.success = True
00142         
00143 
00144     def test_link_pose(self):
00145         print "LNK\n"
00146         rospy.init_node(NAME, anonymous=True)
00147         self.link_pose_topic = rospy.get_param(self.link_pose_topic_name,self.link_pose_topic);
00148         self.valid_pose_topic = rospy.get_param(self.valid_pose_topic_name,self.valid_pose_topic);
00149         self.min_link_samples = rospy.get_param(self.min_link_samples_topic,self.min_link_samples);
00150         self.min_valid_samples = rospy.get_param(self.min_valid_samples_topic,self.min_valid_samples);
00151         self.tolerance = rospy.get_param(self.tolerance_topic,self.tolerance);
00152         self.max_error = rospy.get_param(self.max_error_topic,self.max_error);
00153         self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration);
00154         self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time);
00155 
00156         while not rospy.is_shutdown() and time.time() < self.test_start_time:
00157             rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time)
00158             time.sleep(0.1)
00159 
00160         print "subscribe"
00161         rospy.Subscriber(self.link_pose_topic, Odometry, self.linkP3dInput)
00162         rospy.Subscriber(self.valid_pose_topic, Odometry, self.validP3dInput)
00163 
00164         start_t = time.time()
00165         timeout_t = start_t + self.test_duration
00166         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
00167             self.checkPose()
00168             time.sleep(0.1)
00169         self.assert_(self.success)
00170         
00171 if __name__ == '__main__':
00172     print "Waiting for test to start at time "
00173     rostest.run(PKG, sys.argv[0], LinkPoseTest, sys.argv) #, text_mode=True)
00174 
00175 


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25