00001 """Helper functions for the automatic evaluation of RotorS datasets."""
00002
00003 import optparse
00004 from rosbag_tools import analyze_bag
00005
00006
00007 __author__ = "Fadri Furrer, Michael Burri, Markus Achtelik"
00008 __copyright__ = ("Copyright 2015, Fadri Furrer & Michael Burri & "
00009 "Markus Achtelik, ASL, ETH Zurich, Switzerland")
00010 __credits__ = ["Fadri Furrer", "Michael Burri", "Markus Achtelik"]
00011 __license__ = "ASL 2.0"
00012 __version__ = "0.1"
00013 __maintainer__ = "Fadri Furrer"
00014 __email__ = "fadri.furrer@mavt.ethz.ch"
00015 __status__ = "Development"
00016
00017
00018 def get_score(evaluated_variable, max_variable, scores):
00019 """
00020 Calculate the score from the evaluated_variable.
00021
00022 Args:
00023 evaluated_variable: Float of the variable to be scored.
00024 max_varibale: Float of the maximum value that needs to be undershoot to
00025 score.
00026 scores: List of Floats of the returned scores (length should be 4).
00027 Returns:
00028 The score, that one gets for the evaluated_variable.
00029 """
00030 if evaluated_variable > max_variable:
00031 return scores[0]
00032 elif evaluated_variable > 0.5 * max_variable:
00033 return scores[1]
00034 elif evaluated_variable > 0.1 * max_variable:
00035 return scores[2]
00036 else:
00037 return scores[3]
00038
00039
00040 def initialize():
00041 """ Read the command line params, and create an analyze object. """
00042 default_twist_topic = "/ground_truth/twist"
00043 default_pose_topic = "/ground_truth/pose"
00044 default_save_plots = "false"
00045 default_show_plots = "true"
00046 default_first_waypoint_delay = 5
00047 default_begin_time = 0.0
00048 default_end_time = 1000.0
00049 default_rms_calc_time = 10.0
00050 default_settling_radius = 0.1
00051 default_min_settled_time = 3
00052 default_mav_name = ""
00053 default_motor_velocity_topic = "/motors"
00054 default_waypoint_topic = "/command/trajectory"
00055 default_wrench_topic = "/wrench"
00056
00057 parser = optparse.OptionParser(
00058 """usage: %prog -b bagfile -p topic1 -t topic2
00059
00060 e.g.:
00061 %prog -b /home/username/.ros/firefly.bag
00062 --mav_name firefly
00063 or
00064 %prog -b /Users/username/.ros/hummingbird.bag
00065 --mav_name hummingbird --save_plots True
00066 --delay_first_evaluation 3
00067 """)
00068 parser.add_option(
00069 "-b", "--bagfile",
00070 dest="bagfile",
00071 type="string",
00072 help="Specify the bag file you want to analyze, with the path.")
00073 parser.add_option(
00074 "-p", "--pose_topic",
00075 dest="pose_topic",
00076 default=default_pose_topic,
00077 type="string",
00078 help="The pose topic that you want to extract from the bag file.")
00079 parser.add_option(
00080 "-t", "--twist_topic",
00081 dest="twist_topic",
00082 default=default_twist_topic,
00083 type="string",
00084 help="The twist topic that you want to extract from the bag file.")
00085 parser.add_option(
00086 "-s", "--save_plots",
00087 action="store_true",
00088 dest="save_plots",
00089 default=default_save_plots,
00090 help="Store all the plots in the current directory.")
00091 parser.add_option(
00092 "--prefix",
00093 dest="prefix",
00094 default=None,
00095 type="string",
00096 help="The prefix of the output png files.")
00097 parser.add_option(
00098 "-d", "--plot",
00099 dest="plot",
00100 default=default_show_plots,
00101 type="string",
00102 help="Set to True if you want the data plotted.")
00103 parser.add_option(
00104 "--begin_time",
00105 dest="begin",
00106 default=default_begin_time,
00107 type="float",
00108 help="Start time of the evaluated data.")
00109 parser.add_option(
00110 "-e", "--end_time",
00111 dest="end",
00112 default=default_end_time,
00113 type="float",
00114 help="End time of the evaluated data.")
00115 parser.add_option(
00116 "--rms_calc_time",
00117 dest="rms_calc_time",
00118 default=default_rms_calc_time,
00119 type="float",
00120 help="The length of the RMS evaluation period.")
00121 parser.add_option(
00122 "--settling_radius",
00123 dest="settling_radius",
00124 default=default_settling_radius,
00125 type="float",
00126 help="The radius of the bounding sphere where the system should "
00127 "settle.")
00128 parser.add_option(
00129 "--min_settled_time",
00130 dest="min_settled_time",
00131 default=default_min_settled_time,
00132 type="float",
00133 help="The minimal time for which the system should stay bounded.")
00134 parser.add_option(
00135 "-m", "--motor_velocity_topic",
00136 dest="motor_velocity_topic",
00137 default=default_motor_velocity_topic,
00138 type="string",
00139 help="The motor_velocity topic that you want to extract from the bag "
00140 "file.")
00141 parser.add_option(
00142 "-w", "--waypoint_topic",
00143 dest="waypoint_topic",
00144 default=default_waypoint_topic,
00145 type="string",
00146 help="The waypoint topic that you want to extract from the bag file.")
00147 parser.add_option(
00148 "-W", "--wrench_topic",
00149 dest="wrench_topic",
00150 default=default_wrench_topic,
00151 type="string",
00152 help="The wrench topic that you want to extract from the bag file.")
00153 parser.add_option(
00154 "-D", "--delay_first_evaluation",
00155 dest="first_waypoint_delay",
00156 default=default_first_waypoint_delay,
00157 type="float",
00158 help="The time when the evaluation should start after the first "
00159 "waypoint got published.")
00160 parser.add_option(
00161 "-n", "--mav_name",
00162 dest="mav_name",
00163 default=default_mav_name,
00164 type="string",
00165 help="The name of your MAV (should correspond to the namespace).")
00166
00167 (options, args) = parser.parse_args()
00168 if not options.bagfile:
00169 parser.error('Bagfile not given.')
00170 mav_name = options.mav_name
00171 topic_prefix = mav_name
00172 save_plots = options.save_plots
00173 prefix = options.prefix
00174 bagfile = options.bagfile
00175 pose_topics = analyze_bag.create_topic_list(
00176 topic_prefix + options.pose_topic)
00177 twist_topics = analyze_bag.create_topic_list(
00178 topic_prefix + options.twist_topic)
00179 motor_velocity_topics = analyze_bag.create_topic_list(
00180 topic_prefix + options.motor_velocity_topic)
00181 waypoint_topics = analyze_bag.create_topic_list(
00182 topic_prefix + options.waypoint_topic)
00183 wrench_topics = analyze_bag.create_topic_list(
00184 topic_prefix + options.wrench_topic)
00185 plot = True if (options.plot and options.plot.lower() == 'true') else False
00186 if save_plots:
00187 plot = True
00188 begin_time = options.begin
00189 total_end_time = options.end
00190 rms_calc_time = options.rms_calc_time
00191 settling_radius = options.settling_radius
00192 min_settled_time = options.min_settled_time
00193 first_waypoint_evaluation_delay = options.first_waypoint_delay
00194
00195
00196 ab = analyze_bag.AnalyzeBag(bag_path_name=bagfile, save_plots=save_plots,
00197 prefix=prefix)
00198
00199
00200 for pose_topic in pose_topics:
00201 ab.add_pose_topic(pose_topic)
00202 for twist_topic in twist_topics:
00203 ab.add_twist_topic(twist_topic)
00204 for motor_velocity_topic in motor_velocity_topics:
00205 ab.add_motor_velocity_topic(motor_velocity_topic)
00206 for waypoint_topic in waypoint_topics:
00207 ab.add_waypoint_topic(waypoint_topic)
00208 for wrench_topic in wrench_topics:
00209 ab.add_wrench_topic(wrench_topic)
00210 if len(ab.topics):
00211 ab.extract_messages()
00212
00213 return [ab, plot, begin_time, total_end_time, rms_calc_time,
00214 settling_radius, min_settled_time, first_waypoint_evaluation_delay]
00215
00216
00217 def get_evaluation_period(waypoints, index, bag_time_start, bag_time_end,
00218 total_end_time):
00219 begin_time = waypoints.bag_time[index].to_sec() - bag_time_start
00220
00221 if index + 1 < len(waypoints.x):
00222 end_time = waypoints.bag_time[index + 1].to_sec() - bag_time_start
00223 else:
00224 end_time = min(total_end_time, bag_time_end)
00225 if (bag_time_end < end_time):
00226 print("[Waypoint %d]: Stopping evaluation early, as the "
00227 "bag file ends." % index)
00228 return [begin_time, end_time]
00229
00230
00231 def get_settling_time(positions, set_point, radius, min_time, index,
00232 print_output=True):
00233 settling_time = analyze_bag.settling_time(
00234 set_point, positions, radius, min_time)
00235 if settling_time is not None:
00236 if print_output:
00237 print("[Waypoint %d]: Settling time: %.3f s"
00238 % (index, settling_time))
00239 return settling_time
00240 else:
00241 if print_output:
00242 print("[Waypoint %d]: System didn't settle." % index)
00243 return None
00244
00245
00246 def get_rms_position_error(positions, position_set_point, index,
00247 print_output=True):
00248 pos_rms_error = analyze_bag.xyz_rms_error(position_set_point, positions)
00249 if print_output:
00250 print("[Waypoint %d]: Position RMS error: %.3f m"
00251 % (index, pos_rms_error))
00252 return pos_rms_error
00253
00254
00255 def get_rms_angular_velocity_error(angular_velocities,
00256 angular_velocity_set_point, index,
00257 print_output=True):
00258 pqr_rms_error = analyze_bag.xyz_rms_error(angular_velocity_set_point,
00259 angular_velocities)
00260 if print_output:
00261 print("[Waypoint %d]: Angular velocity RMS error: %.3f rad/s"
00262 % (index, pqr_rms_error))
00263 return pqr_rms_error
00264
00265
00266 def plot_positions(analyze_bag, start_time, end_time, settling_time,
00267 settling_radius, set_point_position, x_range, plot_suffix):
00268 try:
00269 absolute_settling_time = settling_time + start_time
00270 except:
00271 absolute_settling_time = None
00272 analyze_bag.plot_positions(
00273 start_time=start_time,
00274 end_time=end_time,
00275 settling_time=absolute_settling_time,
00276 x_range=x_range,
00277 plot_suffix=plot_suffix)
00278 analyze_bag.plot_position_error(
00279 set_point=set_point_position,
00280 settling_radius=settling_radius,
00281 start_time=start_time,
00282 end_time=end_time,
00283 settling_time=absolute_settling_time,
00284 x_range=x_range,
00285 y_range=[0, 1],
00286 plot_suffix=plot_suffix)
00287
00288
00289 def plot_angular_velocities(analyze_bag, start_time, end_time, settling_time,
00290 x_range, plot_suffix):
00291 try:
00292 absolute_settling_time = settling_time + start_time
00293 except:
00294 absolute_settling_time = None
00295 analyze_bag.plot_angular_velocities(
00296 start_time=start_time,
00297 end_time=end_time,
00298 settling_time=absolute_settling_time,
00299 x_range=x_range,
00300 plot_suffix=plot_suffix,
00301 y_range=[-1.5, 1.5])
00302
00303
00304 def calculate_average(list_values):
00305 try:
00306 return sum(list_values) / len(list_values)
00307 except ZeroDivisionError:
00308 return None
00309
00310
00311 def no_collisions_occured(analyze_bag, start_time, end_time):
00312 try:
00313 collisions = analyze_bag.get_collisions(start_time, end_time)
00314 except:
00315 raise
00316 collisions = []
00317 print("\n")
00318 if len(collisions):
00319 t_last = -1.0
00320 for t in collisions:
00321 if(t - t_last >= 1):
00322 print("Collision at time %.3f!" % t)
00323 t_last = t
00324 print("One or more collisions occurred - no points are awarded!")
00325 return False
00326 else:
00327 print("No collisions occurred - good job!")
00328 return True
00329
00330
00331 def print_scoring(average_value, max_value, value_type, unit, scores):
00332 if average_value is not None:
00333 score = get_score(average_value, max_value, scores)
00334 print("Average %s: %.3f %s" % (value_type, average_value, unit))
00335 print("Score for %s: %.2f" % (value_type, score))
00336 else:
00337 print("No %s evaluated, hence no scoring is provided." % value_type)