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 %s", 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 fiel detected")
for waypoint in read_new_mission(f):
wps.append(waypoint)
rospy.logdebug(waypoint)
elif mission_ext == '.txt':
rospy.loginfo("old style mission fiel 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("wait until ready")
self.wait_until_ready()
rospy.loginfo("send mission")
res = self._srv_wp_push(wps)
rospy.loginfo(res)
self.assertTrue(
res.success,
"(%s) mission could not be transfered" % self.mission_name)
rospy.loginfo("run mission")
self.run_mission()
index = 0
for waypoint in 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.home_alt
self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600,
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, 600, index)
# after reaching position, wait for landing detection if applicable
if waypoint.command == 85 or waypoint.command == 21:
self.wait_on_landing(600, index)
index += 1
rospy.loginfo("mission done, calculating performance metrics")
last_log = get_last_log()
rospy.loginfo("log file %s", last_log)
data = px4tools.ulog.read_ulog(last_log).resample_and_concat(0.1)
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 %s", 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 fiel detected")
for waypoint in read_new_mission(f):
wps.append(waypoint)
rospy.logdebug(waypoint)
elif mission_ext == '.txt':
rospy.loginfo("old style mission fiel 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("wait until ready")
self.wait_until_ready()
rospy.loginfo("send mission")
res = self._srv_wp_push(wps)
rospy.loginfo(res)
self.assertTrue(res.success, "(%s) mission could not be transfered" % self.mission_name)
rospy.loginfo("run mission")
self.run_mission()
index = 0
for waypoint in 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.home_alt
self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, 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, 600, index)
# after reaching position, wait for landing detection if applicable
if waypoint.command == 85 or waypoint.command == 21:
self.wait_on_landing(600, index)
index += 1
rospy.loginfo("mission done, calculating performance metrics")
last_log = get_last_log()
rospy.loginfo("log file %s", last_log)
data = px4tools.ulog.read_ulog(last_log).resample_and_concat(0.1)
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))
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))
