disturbance_eval.py

Go to the documentation of this file.

#!/usr/bin/env python
# This is needed such that int / int gives a float instead of an int
from __future__ import division

import roslib
roslib.load_manifest('rotors_evaluation')

from rosbag_tools import analyze_bag, helpers


__author__ = "Fadri Furrer, Michael Burri, Markus Achtelik"
__copyright__ = ("Copyright 2015, Fadri Furrer & Michael Burri & "
                 "Markus Achtelik, ASL, ETH Zurich, Switzerland")
__credits__ = ["Fadri Furrer", "Michael Burri", "Markus Achtelik"]
__license__ = "ASL 2.0"
__version__ = "0.1"
__maintainer__ = "Fadri Furrer"
__email__ = "fadri.furrer@mavt.ethz.ch"
__status__ = "Development"


def main():
    [ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
     min_settled_time, first_waypoint_evaluation_delay] = helpers.initialize()

    disturbance_time = 40  # [s]
    settling_time_max = 10.0  # [s]
    position_error_max = 0.2  # [m]
    angular_velocity_error_max = 0.2  # [rad/s]

    # Get the begin and end times, and the set point from the waypoints.
    waypoints = ab.waypoint[0]
    bag_time_start = ab.bag_time_start.to_sec()
    bag_time_end = ab.bag_time_end.to_sec()

    print("\n")
    [begin_time, end_time] = helpers.get_evaluation_period(
        waypoints, 0, bag_time_start, bag_time_end, total_end_time)
    print("[Waypoint %d]: [%.3f, %.3f, %.3f, %.3f] at time %.3f s" % (
        0, waypoints.x[0], waypoints.y[0], waypoints.z[0],
        waypoints.yaw[0], begin_time))

    set_point_pos = analyze_bag.create_set_point(
        waypoints.x[0], waypoints.y[0], waypoints.z[0])
    set_point_pqr = analyze_bag.create_set_point(0, 0, 0)

    # TODO(ff): Automatically extract the disturbance time from a topic.
    begin_time = disturbance_time

    # Get all positions for the evaluation period.
    positions = ab.pos[0].slice(begin_time, end_time)

    # Get the time at which the MAV stayed for at least
    # min_settled_time seconds within a ball of settling_radius meters
    # around set_point_pos.
    settling_time = helpers.get_settling_time(
        positions, set_point_pos, settling_radius, min_settled_time, 0, False)

    rms_evaluation_start_time = begin_time
    rms_evaluation_end_time = min(begin_time + rms_calc_time, end_time)
    if settling_time is not None and settling_time < settling_time_max:
        rms_evaluation_start_time = begin_time + settling_time
        rms_evaluation_end_time = min(begin_time + settling_time +
                                      rms_calc_time, end_time)
        # Get the position RMS errors
        rms_positions = ab.pos[0].slice(
            rms_evaluation_start_time, rms_evaluation_end_time)
        pos_rms_error = helpers.get_rms_position_error(
            rms_positions, set_point_pos, 0, False)

        # Get the angular velocity RMS errors
        angular_velocities = ab.pqr[0].slice(
            rms_evaluation_start_time, rms_evaluation_end_time)
        pqr_rms_error = helpers.get_rms_angular_velocity_error(
            angular_velocities, set_point_pqr, 0, False)

    else:
        print("System didn't settle in %f seconds." % settling_time_max)

    # Plot pose msg content if there are any pose topics.
    if plot and len(ab.pose_topics) > 0:
        x_range = [begin_time - 2, rms_evaluation_end_time + 2]
        # ab.plot_3d_trajectories()
        helpers.plot_positions(
            ab, begin_time, rms_evaluation_end_time, settling_time,
            settling_radius, set_point_pos, x_range, str(0))
        helpers.plot_angular_velocities(
            ab, begin_time, rms_evaluation_end_time, settling_time,
            x_range, str(0))

    start_collision_time = (waypoints.bag_time[0].to_sec() +
                            first_waypoint_evaluation_delay)
    if (settling_time is not None and settling_time < settling_time_max and
        helpers.no_collisions_occured(ab, start_collision_time,
                                      rms_evaluation_end_time)):
        print("\n")
        helpers.print_scoring(settling_time, settling_time_max,
                              "settling time", "s", [0.0, 1.5, 3.5, 5.0])
        helpers.print_scoring(pos_rms_error, position_error_max,
                              "position RMS error", "m", [0.0, 1.5, 3.5, 5.0])
        helpers.print_scoring(pqr_rms_error, angular_velocity_error_max,
                              "angular velocity RMS error", "rad/s",
                              [0.0, 1.0, 2.0, 3.0])


if __name__ == "__main__":
    main()