test_base.py
Go to the documentation of this file.
1 import unittest
2 import math
3 
4 import rospy
5 from geometry_msgs.msg import Twist,Vector3
6 from nav_msgs.msg import Odometry
7 
8 class E:
9  def __init__(self,x,y,z):
10  self.x = x
11  self.y = y
12  self.z = z
13  def shortest_euler_distance(self, e_from, e_to):
14  # takes two sets of euler angles, finds minimum transform between the two, FIXME: return some hacked norm for now
15  # start from the euler-from, and apply reverse euler-to transform, see how far we are from 0,0,0
16 
17  # convert to rotation matrix
18  phi = e_from.x / 2.0
19  the = e_from.y / 2.0
20  psi = e_from.z / 2.0
21  w1 = math.cos(phi) * math.cos(the) * math.cos(psi) + math.sin(phi) * math.sin(the) * math.sin(psi)
22  x1 = math.sin(phi) * math.cos(the) * math.cos(psi) - math.cos(phi) * math.sin(the) * math.sin(psi)
23  y1 = math.cos(phi) * math.sin(the) * math.cos(psi) + math.sin(phi) * math.cos(the) * math.sin(psi)
24  z1 = math.cos(phi) * math.cos(the) * math.sin(psi) - math.sin(phi) * math.sin(the) * math.cos(psi)
25 
26  # convert to rotation matrix inverse
27  phi = e_to.x / 2.0
28  the = e_to.y / 2.0
29  psi = e_to.z / 2.0
30  w2 = math.cos(phi) * math.cos(the) * math.cos(psi) + math.sin(phi) * math.sin(the) * math.sin(psi)
31  x2 = -(math.sin(phi) * math.cos(the) * math.cos(psi) - math.cos(phi) * math.sin(the) * math.sin(psi))
32  y2 = -(math.cos(phi) * math.sin(the) * math.cos(psi) + math.sin(phi) * math.cos(the) * math.sin(psi))
33  z2 = -(math.cos(phi) * math.cos(the) * math.sin(psi) - math.sin(phi) * math.sin(the) * math.cos(psi))
34 
35  # add the two rotations
36  w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
37  x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
38  y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
39  z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
40 
41  # recover euler angles
42  squ = w * w
43  sqx = x * x
44  sqy = y * y
45  sqz = z * z
46 
47  # Roll
48  self.x = math.atan2(2.0 * (y*z + w*x), squ - sqx - sqy + sqz)
49 
50  # Pitch
51  sarg = -2.0 * (x*z - w * y)
52  if sarg <= -1.0:
53  self.y = -0.5*math.pi
54  else:
55  if sarg >= 1.0:
56  self.y = 0.5*math.pi
57  else:
58  self.y = math.asin(sarg)
59 
60  # Yaw
61  self.z = math.atan2(2.0 * (x*y + w*z), squ + sqx - sqy - sqz)
62 
63 
64 class Q:
65  def __init__(self,x,y,z,u):
66  self.x = x
67  self.y = y
68  self.z = z
69  self.u = u
70  def normalize(self):
71  s = math.sqrt(self.u * self.u + self.x * self.x + self.y * self.y + self.z * self.z)
72  self.u /= s
73  self.x /= s
74  self.y /= s
75  self.z /= s
76  def getEuler(self):
77  self.normalize()
78  squ = self.u
79  sqx = self.x
80  sqy = self.y
81  sqz = self.z
82  # init euler angles
83  vec = E(0,0,0)
84  # Roll
85  vec.x = math.atan2(2 * (self.y*self.z + self.u*self.x), squ - sqx - sqy + sqz);
86  # Pitch
87  vec.y = math.asin(-2 * (self.x*self.z - self.u*self.y));
88  # Yaw
89  vec.z = math.atan2(2 * (self.x*self.y + self.u*self.z), squ + sqx - sqy - sqz);
90  return vec
91 
92 class BaseTest(unittest.TestCase):
93  def __init__(self, *args):
94  super(BaseTest, self).__init__(*args)
95  self.success = False
96 
97  self.odom_xi = 0;
98  self.odom_yi = 0;
99  self.odom_ti = 0;
100  self.odom_ei = E(0,0,0)
101  self.odom_initialized = False;
102 
103  self.odom_x = 0;
104  self.odom_y = 0;
105  self.odom_t = 0;
106  self.odom_e = E(0,0,0)
107 
108  self.p3d_xi = 0;
109  self.p3d_yi = 0;
110  self.p3d_ti = 0;
111  self.p3d_ei = E(0,0,0)
112  self.p3d_initialized = False;
113 
114  self.p3d_x = 0;
115  self.p3d_y = 0;
116  self.p3d_t = 0;
117  self.p3d_e = E(0,0,0)
118 
119  self.pub = None
120 
121  def normalize_angle_positive(self, angle):
122  return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
123 
124  def normalize_angle(self, angle):
125  anorm = self.normalize_angle_positive(angle)
126  if anorm > math.pi:
127  anorm -= 2*math.pi
128  return anorm
129 
130  def shortest_angular_distance(self, angle_from, angle_to):
131  angle_diff = self.normalize_angle_positive(angle_to) - self.normalize_angle_positive(angle_from)
132  if angle_diff > math.pi:
133  angle_diff = -(2*math.pi - angle_diff)
134  return self.normalize_angle(angle_diff)
135 
136  def printBaseOdom(self, odom):
137  orientation = odom.pose.pose.orientation
138  q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
139  q.normalize()
140  print "odom received"
141  print "odom pos " + "x: " + str(odom.pose.pose.position.x)
142  print "odom pos " + "y: " + str(odom.pose.pose.position.y)
143  print "odom pos " + "t: " + str(q.getEuler().z)
144  print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
145  print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
146  print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
147 
148  def printBaseP3D(self, p3d):
149  print "base pose ground truth received"
150  print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x)
151  print " " + "y: " + str(p3d.pose.pose.position.y)
152  print " " + "z: " + str(p3d.pose.pose.position.z)
153  print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x)
154  print " " + "y: " + str(p3d.pose.pose.orientation.y)
155  print " " + "z: " + str(p3d.pose.pose.orientation.z)
156  print " " + "w: " + str(p3d.pose.pose.orientation.w)
157  print "P3D rate translan: " + "x: " + str(p3d.twist.twist.linear.x)
158  print " " + "y: " + str(p3d.twist.twist.linear.y)
159  print " " + "z: " + str(p3d.twist.twist.linear.z)
160  print "P3D rate rotation: " + "x: " + str(p3d.twist.twist.angular.x)
161  print " " + "y: " + str(p3d.twist.twist.angular.y)
162  print " " + "z: " + str(p3d.twist.twist.angular.z)
163 
164  def odomInput(self, odom):
165  self.printBaseOdom(odom)
166  orientation = odom.pose.pose.orientation
167  q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
168  q.normalize()
169  if self.odom_initialized == False:
170  self.odom_initialized = True
171  self.odom_xi = odom.pose.pose.position.x
172  self.odom_yi = odom.pose.pose.position.y
173  self.odom_ti = q.getEuler().z
174  self.odom_ei = q.getEuler()
175  self.odom_x = odom.pose.pose.position.x - self.odom_xi
176  self.odom_y = odom.pose.pose.position.y - self.odom_yi
177  self.odom_t = q.getEuler().z - self.odom_ti
178  self.odom_e.shortest_euler_distance(self.odom_ei, q.getEuler())
179 
180  def p3dInput(self, p3d):
181  q = Q(p3d.pose.pose.orientation.x , p3d.pose.pose.orientation.y , p3d.pose.pose.orientation.z , p3d.pose.pose.orientation.w)
182  q.normalize()
183  v = q.getEuler()
184 
185  if self.p3d_initialized == False:
186  self.p3d_initialized = True
187  self.p3d_xi = p3d.pose.pose.position.x
188  self.p3d_yi = p3d.pose.pose.position.y
189  self.p3d_ti = v.z
190  self.p3d_ei = E(v.x,v.y,v.z)
191 
192  self.p3d_x = p3d.pose.pose.position.x - self.p3d_xi
193  self.p3d_y = p3d.pose.pose.position.y - self.p3d_yi
194  self.p3d_t = v.z - self.p3d_ti
195  self.p3d_e.shortest_euler_distance(self.p3d_ei,E(v.x,v.y,v.z))
196 
197  def debug_e(self):
198  # display what odom thinks
199  print " odom " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
200  # display what ground truth is
201  print " p3d " + " x: " + str(self.p3d_e.x) + " y: " + str(self.p3d_e.y) + " t: " + str(self.p3d_e.t)
202 
203  def debug_pos(self):
204  # display what odom thinks
205  print " odom " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
206  # display what ground truth is
207  print " p3d " + " x: " + str(self.p3d_x) + " y: " + str(self.p3d_y) + " t: " + str(self.p3d_t)
208 
209  def init_ros(self, name):
210  print "LNK\n"
211  self.pub = rospy.Publisher("/base_controller/command", Twist)
212  rospy.Subscriber("/base_pose_ground_truth", Odometry, self.p3dInput)
213  rospy.Subscriber("/base_odometry/odom", Odometry, self.odomInput)
214  rospy.init_node(name, anonymous=True)
def printBaseOdom(self, odom)
Definition: test_base.py:136
def printBaseP3D(self, p3d)
Definition: test_base.py:148
def normalize_angle_positive(self, angle)
Definition: test_base.py:121
def getEuler(self)
Definition: test_base.py:76
def shortest_euler_distance(self, e_from, e_to)
Definition: test_base.py:13
def __init__(self, args)
Definition: test_base.py:93
def debug_pos(self)
Definition: test_base.py:203
def p3dInput(self, p3d)
Definition: test_base.py:180
def normalize(self)
Definition: test_base.py:70
def __init__(self, x, y, z, u)
Definition: test_base.py:65
def shortest_angular_distance(self, angle_from, angle_to)
Definition: test_base.py:130
def __init__(self, x, y, z)
Definition: test_base.py:9
def odomInput(self, odom)
Definition: test_base.py:164
def init_ros(self, name)
Definition: test_base.py:209
def normalize_angle(self, angle)
Definition: test_base.py:124
def debug_e(self)
Definition: test_base.py:197


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