def __init__(self, file=None, data_start_ind=0, data_end_ind=-1, rolling_window_u=100, rolling_window_y=100, Ts=0.001, data='roll', plot=True): """ Constructor @file String of path to ulog file @dt desired sampling time @data_start_ind Starting index of data @data_end_ind Last index of data. -1 means up to last element @data type of data required {'roll', 'pitch', 'yaw'} @rolling_window moving average window length @Ts resampling times in seconds """ self._raw_data = None if file is None: print("Path to ulg file is not provided") return self._raw_data = px.read_ulog(file) self._dt = Ts # will be calculated from data time index, see process_data() self._Ts = Ts self._full_data_length = None # after resampling self._rolling_window_u = rolling_window_u # moving average window length for controller output filtering self._rolling_window_y = rolling_window_y # moving average window length for plant output filtering self._data_start_ind = data_start_ind self._data_end_ind = data_end_ind data_str = ['roll', 'pitch', 'yaw'] if data not in data_str: print( "data is not among ['roll', 'pitch', 'yaw']. Defaulting to 'roll' " ) data = 'roll' self._data_name = data # Returned variables self._t = None # time vector self._u_raw = None # plant input data self._y_raw = None # plant output data self._u_filtered = None self._y_filtered = None self._trimmed_raw_data = None # between _data_start_ind and _data_end_ind self._plot = plot
def test_mission(self): """Test mission""" if len(sys.argv) < 2: self.fail("usage: mission_test.py mission_file") return self.mission_name = sys.argv[1] mission_file = os.path.dirname( os.path.realpath(__file__)) + "/missions/" + sys.argv[1] rospy.loginfo("reading mission {0}".format(mission_file)) try: wps = read_mission(mission_file) except IOError as e: self.fail(e) # make sure the simulation is ready to start the mission self.wait_for_topics(60) self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, 10, -1) self.wait_for_mav_type(10) # push waypoints to FCU and start mission self.send_wps(wps, 30) self.log_topic_vars() self.set_mode("AUTO.MISSION", 5) self.set_arm(True, 5) rospy.loginfo("run mission {0}".format(self.mission_name)) for index, waypoint in enumerate(wps): # only check position for waypoints where this makes sense if (waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL): alt = waypoint.z_alt if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT: alt += self.altitude.amsl - self.altitude.relative self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 60, index) # check if VTOL transition happens if applicable if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF or waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND or waypoint.command == mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION): transition = waypoint.param1 # used by MAV_CMD_DO_VTOL_TRANSITION if waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF: # VTOL takeoff implies transition to FW transition = mavutil.mavlink.MAV_VTOL_STATE_FW if waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: # VTOL land implies transition to MC transition = mavutil.mavlink.MAV_VTOL_STATE_MC self.wait_for_vtol_state(transition, 60, index) # after reaching position, wait for landing detection if applicable if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND or waypoint.command == mavutil.mavlink.MAV_CMD_NAV_LAND): self.wait_for_landed_state( mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, 60, index) self.set_arm(False, 5) self.clear_wps(5) rospy.loginfo("mission done, calculating performance metrics") last_log = get_last_log() rospy.loginfo("log file {0}".format(last_log)) data = px4tools.read_ulog(last_log).concat(dt=0.1) data = px4tools.compute_data(data) res = px4tools.estimator_analysis(data, False) # enforce performance self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res)) self.assertTrue(res['roll_error_std'] < 5.0, str(res)) self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) self.assertTrue(res['yaw_error_std'] < 5.0, str(res))
def test_mission(self): """Test mission""" if len(sys.argv) < 2: self.fail("usage: mission_test.py mission_file") return self.mission_name = sys.argv[1] mission_file = os.path.dirname( os.path.realpath(__file__)) + "/" + sys.argv[1] rospy.loginfo("reading mission {0}".format(mission_file)) wps = [] with open(mission_file, 'r') as f: mission_ext = os.path.splitext(mission_file)[1] if mission_ext == '.mission': rospy.loginfo("new style mission file detected") for waypoint in read_new_mission(f): wps.append(waypoint) rospy.logdebug(waypoint) elif mission_ext == '.txt': rospy.loginfo("old style mission file detected") mission = QGroundControlWP() for waypoint in mission.read(f): wps.append(waypoint) rospy.logdebug(waypoint) else: raise IOError('unknown mission file extension', mission_ext) rospy.loginfo("send mission") result = False try: res = self.wp_push_srv(start_index=0, waypoints=wps) result = res.success except rospy.ServiceException as e: rospy.logerr(e) self.assertTrue( result, "({0}) mission could not be transfered".format(self.mission_name)) # delay starting the mission self.wait_for_topics(30) # make sure the simulation is ready to start the mission self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, 10, -1) rospy.loginfo("seting mission mode") self.set_mode("AUTO.MISSION", 5) rospy.loginfo("arming") self.set_arm(True, 5) rospy.loginfo("run mission") for index, waypoint in enumerate(wps): # only check position for waypoints where this makes sense if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL: alt = waypoint.z_alt if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT: alt += self.altitude.amsl - self.altitude.relative self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 60, index) # check if VTOL transition happens if applicable if waypoint.command == 84 or waypoint.command == 85 or waypoint.command == 3000: transition = waypoint.param1 if waypoint.command == 84: # VTOL takeoff implies transition to FW transition = ExtendedState.VTOL_STATE_FW if waypoint.command == 85: # VTOL takeoff implies transition to MC transition = ExtendedState.VTOL_STATE_MC self.wait_on_transition(transition, 60, index) # after reaching position, wait for landing detection if applicable if waypoint.command == 85 or waypoint.command == 21: self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, 60, index) rospy.loginfo("disarming") self.set_arm(False, 5) rospy.loginfo("mission done, calculating performance metrics") last_log = get_last_log() rospy.loginfo("log file {0}".format(last_log)) data = px4tools.read_ulog(last_log).concat(dt=0.1) data = px4tools.compute_data(data) res = px4tools.estimator_analysis(data, False) # enforce performance self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res)) self.assertTrue(res['roll_error_std'] < 5.0, str(res)) self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) self.assertTrue(res['yaw_error_std'] < 5.0, str(res))