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