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()
26 settling_time_max = 10.0
27 position_error_max = 0.2
28 angular_velocity_error_max = 0.2
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 = []
39 for index
in range(len(waypoints.x)):
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))
49 positions = ab.pos[0].slice(begin_time, end_time)
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)
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)
67 rms_evaluation_start_time = begin_time
68 rms_evaluation_end_time = min(begin_time + rms_calc_time, end_time)
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)
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)
84 settling_time = helpers.get_settling_time(
85 positions, set_point_pos, settling_radius, min_settled_time,
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)
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)
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)
103 list_settling_time.append(settling_time)
104 list_pos_rms.append(pos_rms_error)
105 list_pqr_rms.append(pqr_rms_error)
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)
115 if plot
and len(ab.pose_topics) > 0:
116 x_range = [begin_time - 2, rms_evaluation_end_time + 2]
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,
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)
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)):
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])
143 if __name__ ==
"__main__":