disturbance_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  disturbance_time = 40 # [s]
27  settling_time_max = 10.0 # [s]
28  position_error_max = 0.2 # [m]
29  angular_velocity_error_max = 0.2 # [rad/s]
30 
31  # Get the begin and end times, and the set point from the waypoints.
32  waypoints = ab.waypoint[0]
33  bag_time_start = ab.bag_time_start.to_sec()
34  bag_time_end = ab.bag_time_end.to_sec()
35 
36  print("\n")
37  [begin_time, end_time] = helpers.get_evaluation_period(
38  waypoints, 0, bag_time_start, bag_time_end, total_end_time)
39  print("[Waypoint %d]: [%.3f, %.3f, %.3f, %.3f] at time %.3f s" % (
40  0, waypoints.x[0], waypoints.y[0], waypoints.z[0],
41  waypoints.yaw[0], begin_time))
42 
43  set_point_pos = analyze_bag.create_set_point(
44  waypoints.x[0], waypoints.y[0], waypoints.z[0])
45  set_point_pqr = analyze_bag.create_set_point(0, 0, 0)
46 
47  # TODO(ff): Automatically extract the disturbance time from a topic.
48  begin_time = disturbance_time
49 
50  # Get all positions for the evaluation period.
51  positions = ab.pos[0].slice(begin_time, end_time)
52 
53  # Get the time at which the MAV stayed for at least
54  # min_settled_time seconds within a ball of settling_radius meters
55  # around set_point_pos.
56  settling_time = helpers.get_settling_time(
57  positions, set_point_pos, settling_radius, min_settled_time, 0, False)
58 
59  rms_evaluation_start_time = begin_time
60  rms_evaluation_end_time = min(begin_time + rms_calc_time, end_time)
61  if settling_time is not None and settling_time < settling_time_max:
62  rms_evaluation_start_time = begin_time + settling_time
63  rms_evaluation_end_time = min(begin_time + settling_time +
64  rms_calc_time, end_time)
65  # Get the position RMS errors
66  rms_positions = ab.pos[0].slice(
67  rms_evaluation_start_time, rms_evaluation_end_time)
68  pos_rms_error = helpers.get_rms_position_error(
69  rms_positions, set_point_pos, 0, False)
70 
71  # Get the angular velocity RMS errors
72  angular_velocities = ab.pqr[0].slice(
73  rms_evaluation_start_time, rms_evaluation_end_time)
74  pqr_rms_error = helpers.get_rms_angular_velocity_error(
75  angular_velocities, set_point_pqr, 0, False)
76 
77  else:
78  print("System didn't settle in %f seconds." % settling_time_max)
79 
80  # Plot pose msg content if there are any pose topics.
81  if plot and len(ab.pose_topics) > 0:
82  x_range = [begin_time - 2, rms_evaluation_end_time + 2]
83  # ab.plot_3d_trajectories()
84  helpers.plot_positions(
85  ab, begin_time, rms_evaluation_end_time, settling_time,
86  settling_radius, set_point_pos, x_range, str(0))
87  helpers.plot_angular_velocities(
88  ab, begin_time, rms_evaluation_end_time, settling_time,
89  x_range, str(0))
90 
91  start_collision_time = (waypoints.bag_time[0].to_sec() +
92  first_waypoint_evaluation_delay)
93  if (settling_time is not None and settling_time < settling_time_max and
94  helpers.no_collisions_occured(ab, start_collision_time,
95  rms_evaluation_end_time)):
96  print("\n")
97  helpers.print_scoring(settling_time, settling_time_max,
98  "settling time", "s", [0.0, 1.5, 3.5, 5.0])
99  helpers.print_scoring(pos_rms_error, position_error_max,
100  "position RMS error", "m", [0.0, 1.5, 3.5, 5.0])
101  helpers.print_scoring(pqr_rms_error, angular_velocity_error_max,
102  "angular velocity RMS error", "rad/s",
103  [0.0, 1.0, 2.0, 3.0])
104 
105 
106 if __name__ == "__main__":
107  main()


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