localization_monitor.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 from nav_msgs.msg import Odometry
4 from geometry_msgs.msg import Pose, Point, Quaternion
5 import tf.transformations
6 import math
7 import threading
8 import numpy as np
9 import matplotlib.pyplot as plt
10 
11 
12 def odom_callback(msg):
13  lock.acquire()
14 
15  global exact_pose
16  global is_exact_init
17  exact_pose = msg.pose.pose
18  is_exact_init = True
19 
20  lock.release()
21 
22 
23 def update_monitor():
24 
25  global is_exact_init
26 
27  if not is_exact_init:
28  return
29 
30  # get SLAM estimated pose from transform
31  try:
32  tf_listener.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(3))
33  (slam_trans, slam_rot) = tf_listener.lookupTransform('map', 'base_footprint', rospy.Time(0))
34  except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
35  return
36 
37  slam_pose = Pose()
38  slam_pose.position.x = slam_trans[0]
39  slam_pose.position.y = slam_trans[1]
40  slam_pose.position.z = slam_trans[2]
41  slam_pose.orientation.x = slam_rot[0]
42  slam_pose.orientation.y = slam_rot[1]
43  slam_pose.orientation.z = slam_rot[2]
44  slam_pose.orientation.w = slam_rot[3]
45  slam_roll,slam_pitch,slam_yaw = tf.transformations.euler_from_quaternion(slam_rot)
46  rospy.loginfo('\n---------------------------------------------')
47  rospy.loginfo('\nSLAM pose: ' + str(slam_pose))
48 
49  # global vars
50  lock.acquire()
51 
52  global exact_pose
53  global samples
54  global error_accum_x
55  global error_accum_y
56  global error_accum_z
57  global error_accum_magnitude
58  global error_accum_yaw
59  global data_time
60  global data_error_x
61  global data_error_y
62  global data_error_z
63  global data_error_magnitude
64  global data_error_roll
65  global data_error_pitch
66  global data_error_yaw
67  global data_dev_x
68  global data_dev_y
69  global data_dev_z
70  global data_dev_magnitude
71  global data_dev_yaw
72  global next_plot_time
73 
74  # get exact pose from exact Odometry topic
75  exact_roll,exact_pitch,exact_yaw = tf.transformations.euler_from_quaternion(
76  [exact_pose.orientation.x, exact_pose.orientation.y, exact_pose.orientation.z, exact_pose.orientation.w])
77  rospy.loginfo('\nExact pose: ' + str(exact_pose))
78 
79  # calculate errors
80  error_x = slam_pose.position.x - exact_pose.position.x
81  error_y = slam_pose.position.y - exact_pose.position.y
82  error_z = slam_pose.position.z - exact_pose.position.z
83  error_magnitude = math.sqrt(error_x*error_x + error_y*error_y + error_z*error_z)
84  error_roll = slam_roll - exact_roll
85  error_pitch = slam_pitch - exact_pitch
86  error_yaw = slam_yaw - exact_yaw
87 
88  # calculate standard deviations
89  samples += 1
90  error_accum_x += error_x * error_x
91  error_accum_y += error_y * error_y
92  error_accum_z += error_z * error_z
93  error_accum_magnitude += error_magnitude * error_magnitude
94  error_accum_yaw += error_yaw * error_yaw
95 
96  sdev_x = math.sqrt(error_accum_x / samples)
97  sdev_y = math.sqrt(error_accum_y / samples)
98  sdev_z = math.sqrt(error_accum_z / samples)
99  sdev_magnitude = math.sqrt(error_accum_magnitude / samples)
100  sdev_yaw = math.sqrt(error_accum_yaw / samples)
101 
102  lock.release()
103 
104  # errors data
105  data_time = np.append(data_time, rospy.Time.now().to_sec())
106  data_error_x = np.append(data_error_x, error_x)
107  data_error_y = np.append(data_error_y, error_y)
108  data_error_z = np.append(data_error_z, error_z)
109  data_error_magnitude = np.append(data_error_magnitude, error_magnitude)
110  # angle errors
111  data_error_roll = np.append(data_error_roll, error_roll)
112  data_error_pitch = np.append(data_error_pitch, error_pitch)
113  data_error_yaw = np.append(data_error_yaw, error_yaw)
114  # standard deviations
115  data_dev_x = np.append(data_dev_x, sdev_x)
116  data_dev_y = np.append(data_dev_y, sdev_y)
117  data_dev_z = np.append(data_dev_z, sdev_z)
118  data_dev_magnitude = np.append(data_dev_magnitude, sdev_magnitude)
119  data_dev_yaw = np.append(data_dev_yaw, sdev_yaw)
120 
121  if samples == 1 or rospy.Time.now() >= next_plot_time:
122  next_plot_time = rospy.Time.now() + rospy.Duration(1)
123  # print results
124  printErrorData()
125  # draw plots
126  error_pos_plot.clear()
127  error_pos_plot.title.set_text("Positin errors: X, Y, Z, Magnitude")
128  error_pos_plot.plot(data_time, data_error_x, 'r-')
129  error_pos_plot.plot(data_time, data_error_y, 'g-')
130  error_pos_plot.plot(data_time, data_error_z, 'b-')
131  error_pos_plot.plot(data_time, data_error_magnitude, 'm-')
132  error_angle_plot.clear()
133  error_angle_plot.title.set_text("Rotation errors: Roll, Pitch, Yaw")
134  error_angle_plot.plot(data_time, data_error_roll, 'r-')
135  error_angle_plot.plot(data_time, data_error_pitch, 'g-')
136  error_angle_plot.plot(data_time, data_error_yaw, 'b-')
137  deviation_plot.clear()
138  deviation_plot.title.set_text("Standard deviations: X, Y, Z, Magnitude")
139  deviation_plot.plot(data_time, data_dev_x, 'r-')
140  deviation_plot.plot(data_time, data_dev_y, 'g-')
141  deviation_plot.plot(data_time, data_dev_z, 'b-')
142  deviation_plot.plot(data_time, data_dev_magnitude, 'm-')
143  # histograms
144  pos_hist.clear()
145  pos_hist.title.set_text("Position error: X")
146  pos_hist.hist(data_error_x, bins='auto')
147  pos_magnitude_hist.clear()
148  pos_magnitude_hist.title.set_text("Position error: Magnitude")
149  pos_magnitude_hist.hist(data_error_magnitude, bins='auto')
150 
151  fig.canvas.draw()
152  fig.canvas.flush_events()
153 
154 
155 def printErrorData():
156  rospy.loginfo("\n====================\nLocalization data:" +
157  "\nPosition errors - Magnitude: " + str(data_error_magnitude[-1]) + ", X: " + str(data_error_x[-1]) +
158  ", Y: " + str(data_error_y[-1]) + ", Z: " + str(data_error_z[-1]) +
159  "\nRotation errors - Roll: " + str(data_error_roll[-1]) + ", Pitch: " + str(data_error_pitch[-1]) +
160  ", Yaw: " + str(data_error_yaw[-1]) +
161  "\nStandard deviations - Magnitude: " + str(data_dev_magnitude[-1]) + ", X: " + str(data_dev_x[-1]) +
162  ", Y: " + str(data_dev_y[-1]) + ", Z: " + str(data_dev_z[-1]) +
163  "\n====================")
164 
165 
166 if __name__ == '__main__':
167 
168  global exact_pose
169  global is_exact_init
170  global samples
171  global error_accum_x
172  global error_accum_y
173  global error_accum_z
174  global error_accum_magnitude
175  global error_accum_yaw
176  global data_time
177  global data_error_x
178  global data_error_y
179  global data_error_z
180  global data_error_magnitude
181  global data_error_roll
182  global data_error_pitch
183  global data_error_yaw
184  global data_dev_x
185  global data_dev_y
186  global data_dev_z
187  global data_dev_magnitude
188  global data_dev_yaw
189 
190  lock = threading.Lock()
191  exact_pose = Odometry()
192  is_exact_init = False
193 
194  samples = 0
195  error_accum_x = 0.0
196  error_accum_y = 0.0
197  error_accum_z = 0.0
198  error_accum_magnitude = 0.0
199  error_accum_yaw = 0.0
200 
201  data_time = np.array([])
202  data_error_x = np.array([])
203  data_error_y = np.array([])
204  data_error_z = np.array([])
205  data_error_magnitude = np.array([])
206  data_error_roll = np.array([])
207  data_error_pitch = np.array([])
208  data_error_yaw = np.array([])
209  data_dev_x = np.array([])
210  data_dev_y = np.array([])
211  data_dev_z = np.array([])
212  data_dev_magnitude = np.array([])
213  data_dev_yaw = np.array([])
214 
215  rospy.init_node('localization_monitor', anonymous=True)
216  tf_listener = tf.TransformListener()
217  rospy.Subscriber('/exact_pose', Odometry, odom_callback)
218 
219  plt.ion()
220  fig = plt.figure()
221  fig.canvas.set_window_title('Localization monitor')
222  error_pos_plot = fig.add_subplot(321)
223  error_angle_plot = fig.add_subplot(323)
224  deviation_plot = fig.add_subplot(325)
225  pos_hist = fig.add_subplot(322)
226  pos_magnitude_hist = fig.add_subplot(324)
227  fig.show()
228 
229  # print errors data on exit
230  rospy.on_shutdown(printErrorData)
231  # run update loop
232  r = rospy.Rate(5) # 5hz
233  while not rospy.is_shutdown():
234  update_monitor()
235  r.sleep()
localization_monitor.odom_callback
def odom_callback(msg)
Definition: localization_monitor.py:12
localization_monitor.update_monitor
def update_monitor()
Definition: localization_monitor.py:23
localization_monitor.printErrorData
def printErrorData()
Definition: localization_monitor.py:155

magni_nav
Author(s):
autogenerated on Tue Jun 1 2021 02:33:39