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])
def main():
    [
        ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
        min_settled_time, first_waypoint_evaluation_delay
    ] = helpers.initialize()

    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()
    list_settling_time = []
    list_pos_rms = []
    list_pqr_rms = []

    # Iterate over waypoints, and create evaluations for each waypoint.
    for index in range(len(waypoints.x)):
        settling_time = None
        print("\n")
        [begin_time, end_time
         ] = helpers.get_evaluation_period(waypoints, index, bag_time_start,
                                           bag_time_end, total_end_time)
        print("[Waypoint %d]: [%.3f, %.3f, %.3f, %.3f] at time %.3f s" %
              (index, waypoints.x[index], waypoints.y[index],
               waypoints.z[index], waypoints.yaw[index], begin_time))

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

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

        # Treat the first waypoint specially, as the MAV will most likely still
        # be in collision (with the ground), as the first waypoint gets
        # published. You can specify the delay of the evaluation with the
        # first_waypoint_evaluation_delay option.
        if index == 0:
            begin_time += first_waypoint_evaluation_delay
            print("Setting the first evaluation start time %f s after the "
                  "first waypoint was published.\n" %
                  first_waypoint_evaluation_delay)
            print("[Waypoint %d]: Settling time for this waypoint not\n"
                  "              considered." % index)

            rms_evaluation_start_time = begin_time
            rms_evaluation_end_time = min(begin_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, index)

            # 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, index)
        else:
            # 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, index)
            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, index)

                # 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, index)

                list_settling_time.append(settling_time)
                list_pos_rms.append(pos_rms_error)
                list_pqr_rms.append(pqr_rms_error)
            else:
                print("[Waypoint %d]: System didn't settle  in %f seconds -- "
                      "inserting 101 % of defined maximum values." %
                      (index, settling_time_max))
                list_settling_time.append(settling_time_max * 1.01)
                list_pos_rms.append(position_error_max * 1.01)
                list_pqr_rms.append(angular_velocity_error_max * 1.01)

        # 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(index))
            helpers.plot_angular_velocities(ab, begin_time,
                                            rms_evaluation_end_time,
                                            settling_time, x_range, str(index))

    average_settling_time = helpers.calculate_average(list_settling_time)
    average_pos_rms = helpers.calculate_average(list_pos_rms)
    average_pqr_rms = helpers.calculate_average(list_pqr_rms)

    start_collision_time = (waypoints.bag_time[0].to_sec() +
                            first_waypoint_evaluation_delay)
    if (helpers.no_collisions_occured(ab, start_collision_time,
                                      rms_evaluation_end_time)):
        print("\n")
        helpers.print_scoring(average_settling_time, settling_time_max,
                              "settling time", "s", [0.0, 1.5, 3.5, 5.0])
        helpers.print_scoring(average_pos_rms, position_error_max,
                              "position RMS error", "m", [0.0, 1.5, 3.5, 5.0])
        helpers.print_scoring(average_pqr_rms, angular_velocity_error_max,
                              "angular velocity RMS error", "rad/s",
                              [0.0, 1.0, 2.0, 3.0])
Exemple #3
0
def main():
    [
        ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
        min_settled_time, first_waypoint_evaluation_delay
    ] = helpers.initialize()

    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()

    settling_time = None
    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)

    begin_time += first_waypoint_evaluation_delay
    print("Setting the evaluation start time %f s after the "
          "waypoint was published." % first_waypoint_evaluation_delay)

    rms_evaluation_start_time = begin_time
    rms_evaluation_end_time = min(begin_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,
                                                   print_output=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,
                                                           print_output=False)

    # 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 (helpers.no_collisions_occured(ab, start_collision_time,
                                      rms_evaluation_end_time)):
        print("\n")
        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])
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])
def main():
    [ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
     min_settled_time, first_waypoint_evaluation_delay] = helpers.initialize()

    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()

    settling_time = None
    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)

    begin_time += first_waypoint_evaluation_delay
    print("Setting the evaluation start time %f s after the "
          "waypoint was published."
          % first_waypoint_evaluation_delay)

    rms_evaluation_start_time = begin_time
    rms_evaluation_end_time = min(begin_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, print_output=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, print_output=False)

    # 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 (helpers.no_collisions_occured(ab, start_collision_time,
                                      rms_evaluation_end_time)):
        print("\n")
        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])
def main():
    [ab, plot, begin_time, total_end_time, rms_calc_time, settling_radius,
     min_settled_time, first_waypoint_evaluation_delay] = helpers.initialize()

    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()
    list_settling_time = []
    list_pos_rms = []
    list_pqr_rms = []

    # Iterate over waypoints, and create evaluations for each waypoint.
    for index in range(len(waypoints.x)):
        settling_time = None
        print("\n")
        [begin_time, end_time] = helpers.get_evaluation_period(
            waypoints, index, bag_time_start, bag_time_end, total_end_time)
        print("[Waypoint %d]: [%.3f, %.3f, %.3f, %.3f] at time %.3f s" % (
            index, waypoints.x[index], waypoints.y[index], waypoints.z[index],
            waypoints.yaw[index], begin_time))

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

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

        # Treat the first waypoint specially, as the MAV will most likely still
        # be in collision (with the ground), as the first waypoint gets
        # published. You can specify the delay of the evaluation with the
        # first_waypoint_evaluation_delay option.
        if index == 0:
            begin_time += first_waypoint_evaluation_delay
            print("Setting the first evaluation start time %f s after the "
                  "first waypoint was published.\n"
                  % first_waypoint_evaluation_delay)
            print("[Waypoint %d]: Settling time for this waypoint not\n"
                  "              considered." % index)

            rms_evaluation_start_time = begin_time
            rms_evaluation_end_time = min(begin_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, index)

            # 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, index)
        else:
            # 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,
                index)
            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, index)

                # 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, index)

                list_settling_time.append(settling_time)
                list_pos_rms.append(pos_rms_error)
                list_pqr_rms.append(pqr_rms_error)
            else:
                print("[Waypoint %d]: System didn't settle  in %f seconds -- "
                      "inserting 101 % of defined maximum values."
                      % (index, settling_time_max))
                list_settling_time.append(settling_time_max * 1.01)
                list_pos_rms.append(position_error_max * 1.01)
                list_pqr_rms.append(angular_velocity_error_max * 1.01)

        # 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(index))
            helpers.plot_angular_velocities(
                ab, begin_time, rms_evaluation_end_time, settling_time,
                x_range, str(index))

    average_settling_time = helpers.calculate_average(list_settling_time)
    average_pos_rms = helpers.calculate_average(list_pos_rms)
    average_pqr_rms = helpers.calculate_average(list_pqr_rms)

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