iri_common_smach: homogeneous_product

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('estirabot_apps')
00003 import rospy
00004 import smach
00005 import smach_ros
00006 import threading
00007 import time
00008 import tf
00009 import geometry_msgs
00010 import unittest
00011 
00012 from estirabot_common import *
00013 from geometry_msgs.msg import *
00014 from pprint import pprint
00015 
00016 import sys
00017 
00018 ## A sample python unit test
00019 class TestHomogeneousTranform(unittest.TestCase):
00020     def setUp(self):
00021         self.pose        = geometry_msgs.msg.Pose()
00022         self.transform   = geometry_msgs.msg.Transform()
00023         self.expected    = geometry_msgs.msg.Pose()
00024         self.pose_result = geometry_msgs.msg.Pose()
00025 
00026     # stupid python ... why foo is needed here?
00027     def execute(self):
00028         self.pose_result = homogeneous_product_pose_transform(self.pose, self.transform)
00029         self.check_pose_equals(self.pose_result, self.expected)
00030 
00031     def check_pose_equals(self, pose1, pose2):
00032         self.assertAlmostEqual(pose1.orientation.x,pose2.orientation.x)
00033         self.assertAlmostEqual(pose1.orientation.y,pose2.orientation.y)
00034         self.assertAlmostEqual(pose1.orientation.z,pose2.orientation.z)
00035         self.assertAlmostEqual(pose1.orientation.w,pose2.orientation.w)
00036         self.assertAlmostEqual(pose1.position.x,pose2.position.x)
00037         self.assertAlmostEqual(pose1.position.y,pose2.position.y)
00038         self.assertAlmostEqual(pose1.position.z,pose2.position.z)
00039 
00040 class TestHomogeneousTranformOnlyRotation(TestHomogeneousTranform):
00041     def test_run(self):
00042         self.pose.orientation.x = 0.0
00043         self.pose.orientation.y = 0.0
00044         self.pose.orientation.z = 0.0
00045         self.pose.orientation.w = 1.0
00046         self.pose.position.x = 5.0
00047         self.pose.position.y = 4.0
00048         self.pose.position.z = 3.0
00049 
00050         self.transform.rotation.x = 1.0
00051         self.transform.rotation.y = 0.0
00052         self.transform.rotation.z = 0.0
00053         self.transform.rotation.w = 0.0
00054 
00055         self.expected.position      = self.pose.position
00056         self.expected.orientation.x = 1.0
00057         self.expected.orientation.y = 0.0
00058         self.expected.orientation.z = 0.0
00059         self.expected.orientation.w = 0.0
00060 
00061         self.execute()
00062 
00063 class TestHomogeneousTranformOnlyTranslation(TestHomogeneousTranform):
00064     def test_run(self):
00065         self.pose.orientation.x = 0.0
00066         self.pose.orientation.y = 0.0
00067         self.pose.orientation.z = 0.0
00068         self.pose.orientation.w = 1.0
00069         self.pose.position.x = 5.0
00070         self.pose.position.y = 4.0
00071         self.pose.position.z = 3.0
00072 
00073         self.transform.translation.x = 2.0
00074         self.transform.translation.y = 2.0
00075         self.transform.translation.z = 2.0
00076         self.transform.rotation.x = 0.0
00077         self.transform.rotation.y = 0.0
00078         self.transform.rotation.z = 0.0
00079         self.transform.rotation.w = 1.0
00080 
00081         self.expected.orientation   = self.pose.orientation
00082         self.expected.position.x = 7.0
00083         self.expected.position.y = 6.0
00084         self.expected.position.z = 5.0
00085 
00086         self.execute()
00087 
00088 if __name__ == '__main__':
00089     import rostest
00090     rostest.rosrun('estirabot_apps', 'testHomogeneoustranformOnlyrotation', 
00091                                       TestHomogeneousTranformOnlyRotation)
00092     rostest.rosrun('estirabot_apps', 'testHomogeneoustranformonlytranslation', 
00093                                       TestHomogeneousTranformOnlyTranslation)