test_base.py
Go to the documentation of this file.
00001 import unittest
00002 import math
00003 
00004 import rospy
00005 from geometry_msgs.msg import Twist,Vector3
00006 from nav_msgs.msg import Odometry
00007 
00008 class E:
00009     def __init__(self,x,y,z):
00010         self.x = x
00011         self.y = y
00012         self.z = z
00013     def shortest_euler_distance(self, e_from, e_to):
00014         # takes two sets of euler angles, finds minimum transform between the two, FIXME: return some hacked norm for now
00015         # start from the euler-from, and apply reverse euler-to transform, see how far we are from 0,0,0
00016 
00017         # convert to rotation matrix
00018         phi = e_from.x / 2.0
00019         the = e_from.y / 2.0
00020         psi = e_from.z / 2.0
00021         w1 = math.cos(phi) * math.cos(the) * math.cos(psi) + math.sin(phi) * math.sin(the) * math.sin(psi)
00022         x1 = math.sin(phi) * math.cos(the) * math.cos(psi) - math.cos(phi) * math.sin(the) * math.sin(psi)
00023         y1 = math.cos(phi) * math.sin(the) * math.cos(psi) + math.sin(phi) * math.cos(the) * math.sin(psi)
00024         z1 = math.cos(phi) * math.cos(the) * math.sin(psi) - math.sin(phi) * math.sin(the) * math.cos(psi)
00025   
00026         # convert to rotation matrix inverse
00027         phi = e_to.x / 2.0
00028         the = e_to.y / 2.0
00029         psi = e_to.z / 2.0
00030         w2 = math.cos(phi) * math.cos(the) * math.cos(psi) + math.sin(phi) * math.sin(the) * math.sin(psi)
00031         x2 = -(math.sin(phi) * math.cos(the) * math.cos(psi) - math.cos(phi) * math.sin(the) * math.sin(psi))
00032         y2 = -(math.cos(phi) * math.sin(the) * math.cos(psi) + math.sin(phi) * math.cos(the) * math.sin(psi))
00033         z2 = -(math.cos(phi) * math.cos(the) * math.sin(psi) - math.sin(phi) * math.sin(the) * math.cos(psi))
00034   
00035         # add the two rotations
00036         w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
00037         x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
00038         y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
00039         z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
00040   
00041         # recover euler angles
00042         squ = w * w
00043         sqx = x * x
00044         sqy = y * y
00045         sqz = z * z
00046   
00047         # Roll
00048         self.x = math.atan2(2.0 * (y*z + w*x), squ - sqx - sqy + sqz)
00049   
00050         # Pitch
00051         sarg = -2.0 * (x*z - w * y)
00052         if sarg <= -1.0:
00053             self.y = -0.5*math.pi
00054         else:
00055             if sarg >= 1.0:
00056                 self.y = 0.5*math.pi
00057             else:
00058                 self.y = math.asin(sarg)
00059   
00060         # Yaw
00061         self.z = math.atan2(2.0 * (x*y + w*z), squ + sqx - sqy - sqz)
00062 
00063 
00064 class Q:
00065     def __init__(self,x,y,z,u):
00066         self.x = x
00067         self.y = y
00068         self.z = z
00069         self.u = u
00070     def normalize(self):
00071         s = math.sqrt(self.u * self.u + self.x * self.x + self.y * self.y + self.z * self.z)
00072         self.u /= s
00073         self.x /= s
00074         self.y /= s
00075         self.z /= s
00076     def getEuler(self):
00077         self.normalize()
00078         squ = self.u
00079         sqx = self.x
00080         sqy = self.y
00081         sqz = self.z
00082         # init euler angles
00083         vec = E(0,0,0)
00084         # Roll
00085         vec.x = math.atan2(2 * (self.y*self.z + self.u*self.x), squ - sqx - sqy + sqz);
00086         # Pitch
00087         vec.y = math.asin(-2 * (self.x*self.z - self.u*self.y));
00088         # Yaw
00089         vec.z = math.atan2(2 * (self.x*self.y + self.u*self.z), squ + sqx - sqy - sqz);
00090         return vec
00091 
00092 class BaseTest(unittest.TestCase):
00093     def __init__(self, *args):
00094         super(BaseTest, self).__init__(*args)
00095         self.success = False
00096 
00097         self.odom_xi = 0;
00098         self.odom_yi = 0;
00099         self.odom_ti = 0;
00100         self.odom_ei = E(0,0,0)
00101         self.odom_initialized = False;
00102 
00103         self.odom_x = 0;
00104         self.odom_y = 0;
00105         self.odom_t = 0;
00106         self.odom_e = E(0,0,0)
00107         
00108         self.p3d_xi = 0;
00109         self.p3d_yi = 0;
00110         self.p3d_ti = 0;
00111         self.p3d_ei = E(0,0,0)
00112         self.p3d_initialized = False;
00113 
00114         self.p3d_x = 0;
00115         self.p3d_y = 0;
00116         self.p3d_t = 0;
00117         self.p3d_e = E(0,0,0)
00118 
00119         self.pub = None
00120         
00121     def normalize_angle_positive(self, angle):
00122         return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
00123 
00124     def normalize_angle(self, angle):
00125         anorm = self.normalize_angle_positive(angle)
00126         if anorm > math.pi:
00127           anorm -= 2*math.pi
00128         return anorm
00129 
00130     def shortest_angular_distance(self, angle_from, angle_to):
00131         angle_diff = self.normalize_angle_positive(angle_to) - self.normalize_angle_positive(angle_from)
00132         if angle_diff > math.pi:
00133           angle_diff = -(2*math.pi - angle_diff)
00134         return self.normalize_angle(angle_diff)
00135 
00136     def printBaseOdom(self, odom):
00137         orientation = odom.pose.pose.orientation
00138         q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
00139         q.normalize()
00140         print "odom received"
00141         print "odom pos " + "x: " + str(odom.pose.pose.position.x)
00142         print "odom pos " + "y: " + str(odom.pose.pose.position.y)
00143         print "odom pos " + "t: " + str(q.getEuler().z)
00144         print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
00145         print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
00146         print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
00147 
00148     def printBaseP3D(self, p3d):
00149         print "base pose ground truth received"
00150         print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x)
00151         print "                   " + "y: " + str(p3d.pose.pose.position.y)
00152         print "                   " + "z: " + str(p3d.pose.pose.position.z)
00153         print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x)
00154         print "                   " + "y: " + str(p3d.pose.pose.orientation.y)
00155         print "                   " + "z: " + str(p3d.pose.pose.orientation.z)
00156         print "                   " + "w: " + str(p3d.pose.pose.orientation.w)
00157         print "P3D rate translan: " + "x: " + str(p3d.twist.twist.linear.x)
00158         print "                   " + "y: " + str(p3d.twist.twist.linear.y)
00159         print "                   " + "z: " + str(p3d.twist.twist.linear.z)
00160         print "P3D rate rotation: " + "x: " + str(p3d.twist.twist.angular.vx)
00161         print "                   " + "y: " + str(p3d.twist.twist.angular.vy)
00162         print "                   " + "z: " + str(p3d.twist.twist.angular.vz)
00163 
00164     def odomInput(self, odom):
00165         self.printBaseOdom(odom)
00166         orientation = odom.pose.pose.orientation
00167         q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
00168         q.normalize()
00169         if self.odom_initialized == False:
00170             self.odom_initialized = True
00171             self.odom_xi = odom.pose.pose.position.x
00172             self.odom_yi = odom.pose.pose.position.y
00173             self.odom_ti = q.getEuler().z
00174             self.odom_ei = q.getEuler()
00175         self.odom_x = odom.pose.pose.position.x   - self.odom_xi
00176         self.odom_y = odom.pose.pose.position.y   - self.odom_yi
00177         self.odom_t = q.getEuler().z  - self.odom_ti
00178         self.odom_e.shortest_euler_distance(self.odom_ei, q.getEuler())
00179         
00180     def p3dInput(self, p3d):
00181         q = Q(p3d.pose.pose.orientation.x , p3d.pose.pose.orientation.y , p3d.pose.pose.orientation.z , p3d.pose.pose.orientation.w)
00182         q.normalize()
00183         v = q.getEuler()
00184 
00185         if self.p3d_initialized == False:
00186             self.p3d_initialized = True
00187             self.p3d_xi = p3d.pose.pose.position.x
00188             self.p3d_yi = p3d.pose.pose.position.y
00189             self.p3d_ti = v.z
00190             self.p3d_ei = E(v.x,v.y,v.z)
00191             
00192         self.p3d_x = p3d.pose.pose.position.x - self.p3d_xi
00193         self.p3d_y = p3d.pose.pose.position.y - self.p3d_yi
00194         self.p3d_t = v.z                      - self.p3d_ti
00195         self.p3d_e.shortest_euler_distance(self.p3d_ei,E(v.x,v.y,v.z))    
00196 
00197     def debug_e(self):
00198         # display what odom thinks
00199         print " odom    " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
00200         # display what ground truth is
00201         print " p3d     " + " x: " + str(self.p3d_e.x)  + " y: " + str(self.p3d_e.y)  + " t: " + str(self.p3d_e.t)
00202 
00203     def debug_pos(self):
00204         # display what odom thinks
00205         print " odom    " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
00206         # display what ground truth is
00207         print " p3d     " + " x: " + str(self.p3d_x)  + " y: " + str(self.p3d_y)  + " t: " + str(self.p3d_t)
00208 
00209     def init_ros(self, name):
00210         print "LNK\n"
00211         self.pub = rospy.Publisher("/base_controller/command", Twist)
00212         rospy.Subscriber("/base_pose_ground_truth", Odometry, self.p3dInput)
00213         rospy.Subscriber("/base_odometry/odom",     Odometry, self.odomInput)
00214         rospy.init_node(name, anonymous=True)


pr2_gazebo
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 17:49:15