3 from __future__
import division
6 roslib.load_manifest(
'rotors_evaluation')
8 from rosbag_tools
import analyze_bag, helpers
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" 17 __maintainer__ =
"Fadri Furrer" 18 __email__ =
"fadri.furrer@mavt.ethz.ch" 19 __status__ =
"Development" 23 [ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
24 min_settled_time, first_waypoint_evaluation_delay] = helpers.initialize()
27 settling_time_max = 10.0
28 position_error_max = 0.2
29 angular_velocity_error_max = 0.2
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()
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))
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)
48 begin_time = disturbance_time
51 positions = ab.pos[0].slice(begin_time, end_time)
56 settling_time = helpers.get_settling_time(
57 positions, set_point_pos, settling_radius, min_settled_time, 0,
False)
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)
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)
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)
78 print(
"System didn't settle in %f seconds." % settling_time_max)
81 if plot
and len(ab.pose_topics) > 0:
82 x_range = [begin_time - 2, rms_evaluation_end_time + 2]
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,
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)):
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])
106 if __name__ ==
"__main__":