waypoints_eval.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # This is needed such that int / int gives a float instead of an int
3 from __future__ import division
4 
5 import roslib
6 roslib.load_manifest('rotors_evaluation')
7 
8 from rosbag_tools import analyze_bag, helpers
9 
10 
11 __author__ = "Fadri Furrer, Michael Burri, Markus Achtelik"
12 __copyright__ = ("Copyright 2015, Fadri Furrer & Michael Burri & "
13  "Markus Achtelik, ASL, ETH Zurich, Switzerland")
14 __credits__ = ["Fadri Furrer", "Michael Burri", "Markus Achtelik"]
15 __license__ = "ASL 2.0"
16 __version__ = "0.1"
17 __maintainer__ = "Fadri Furrer"
18 __email__ = "fadri.furrer@mavt.ethz.ch"
19 __status__ = "Development"
20 
21 
22 def main():
23  [ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
24  min_settled_time, first_waypoint_evaluation_delay] = helpers.initialize()
25 
26  settling_time_max = 10.0 # [s]
27  position_error_max = 0.2 # [m]
28  angular_velocity_error_max = 0.2 # [rad/s]
29 
30  # Get the begin and end times, and the set point from the waypoints.
31  waypoints = ab.waypoint[0]
32  bag_time_start = ab.bag_time_start.to_sec()
33  bag_time_end = ab.bag_time_end.to_sec()
34  list_settling_time = []
35  list_pos_rms = []
36  list_pqr_rms = []
37 
38  # Iterate over waypoints, and create evaluations for each waypoint.
39  for index in range(len(waypoints.x)):
40  settling_time = None
41  print("\n")
42  [begin_time, end_time] = helpers.get_evaluation_period(
43  waypoints, index, bag_time_start, bag_time_end, total_end_time)
44  print("[Waypoint %d]: [%.3f, %.3f, %.3f, %.3f] at time %.3f s" % (
45  index, waypoints.x[index], waypoints.y[index], waypoints.z[index],
46  waypoints.yaw[index], begin_time))
47 
48  # Get all positions for the evaluation period.
49  positions = ab.pos[0].slice(begin_time, end_time)
50 
51  set_point_pos = analyze_bag.create_set_point(
52  waypoints.x[index], waypoints.y[index], waypoints.z[index])
53  set_point_pqr = analyze_bag.create_set_point(0, 0, 0)
54 
55  # Treat the first waypoint specially, as the MAV will most likely still
56  # be in collision (with the ground), as the first waypoint gets
57  # published. You can specify the delay of the evaluation with the
58  # first_waypoint_evaluation_delay option.
59  if index == 0:
60  begin_time += first_waypoint_evaluation_delay
61  print("Setting the first evaluation start time %f s after the "
62  "first waypoint was published.\n"
63  % first_waypoint_evaluation_delay)
64  print("[Waypoint %d]: Settling time for this waypoint not\n"
65  " considered." % index)
66 
67  rms_evaluation_start_time = begin_time
68  rms_evaluation_end_time = min(begin_time + rms_calc_time, end_time)
69  # Get the position RMS errors.
70  rms_positions = ab.pos[0].slice(rms_evaluation_start_time,
71  rms_evaluation_end_time)
72  pos_rms_error = helpers.get_rms_position_error(
73  rms_positions, set_point_pos, index)
74 
75  # Get the angular velocity RMS errors.
76  angular_velocities = ab.pqr[0].slice(rms_evaluation_start_time,
77  rms_evaluation_end_time)
78  pqr_rms_error = helpers.get_rms_angular_velocity_error(
79  angular_velocities, set_point_pqr, index)
80  else:
81  # Get the time at which the MAV stayed for at least
82  # min_settled_time seconds within a ball of settling_radius meters
83  # around set_point_pos.
84  settling_time = helpers.get_settling_time(
85  positions, set_point_pos, settling_radius, min_settled_time,
86  index)
87  if settling_time is not None and settling_time < settling_time_max:
88  rms_evaluation_start_time = begin_time + settling_time
89  rms_evaluation_end_time = min(begin_time + settling_time +
90  rms_calc_time, end_time)
91  # Get the position RMS errors
92  rms_positions = ab.pos[0].slice(
93  rms_evaluation_start_time, rms_evaluation_end_time)
94  pos_rms_error = helpers.get_rms_position_error(
95  rms_positions, set_point_pos, index)
96 
97  # Get the angular velocity RMS errors
98  angular_velocities = ab.pqr[0].slice(
99  rms_evaluation_start_time, rms_evaluation_end_time)
100  pqr_rms_error = helpers.get_rms_angular_velocity_error(
101  angular_velocities, set_point_pqr, index)
102 
103  list_settling_time.append(settling_time)
104  list_pos_rms.append(pos_rms_error)
105  list_pqr_rms.append(pqr_rms_error)
106  else:
107  print("[Waypoint %d]: System didn't settle in %f seconds -- "
108  "inserting 101 % of defined maximum values."
109  % (index, settling_time_max))
110  list_settling_time.append(settling_time_max * 1.01)
111  list_pos_rms.append(position_error_max * 1.01)
112  list_pqr_rms.append(angular_velocity_error_max * 1.01)
113 
114  # Plot pose msg content if there are any pose topics.
115  if plot and len(ab.pose_topics) > 0:
116  x_range = [begin_time - 2, rms_evaluation_end_time + 2]
117  # ab.plot_3d_trajectories()
118  helpers.plot_positions(
119  ab, begin_time, rms_evaluation_end_time, settling_time,
120  settling_radius, set_point_pos, x_range, str(index))
121  helpers.plot_angular_velocities(
122  ab, begin_time, rms_evaluation_end_time, settling_time,
123  x_range, str(index))
124 
125  average_settling_time = helpers.calculate_average(list_settling_time)
126  average_pos_rms = helpers.calculate_average(list_pos_rms)
127  average_pqr_rms = helpers.calculate_average(list_pqr_rms)
128 
129  start_collision_time = (waypoints.bag_time[0].to_sec() +
130  first_waypoint_evaluation_delay)
131  if (helpers.no_collisions_occured(ab, start_collision_time,
132  rms_evaluation_end_time)):
133  print("\n")
134  helpers.print_scoring(average_settling_time, settling_time_max,
135  "settling time", "s", [0.0, 1.5, 3.5, 5.0])
136  helpers.print_scoring(average_pos_rms, position_error_max,
137  "position RMS error", "m", [0.0, 1.5, 3.5, 5.0])
138  helpers.print_scoring(average_pqr_rms, angular_velocity_error_max,
139  "angular velocity RMS error", "rad/s",
140  [0.0, 1.0, 2.0, 3.0])
141 
142 
143 if __name__ == "__main__":
144  main()


rotors_evaluation
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Mon Feb 28 2022 23:39:00