def read_mission(mission_filename): wps = [] with open(mission_filename, 'r') as f: mission_filename_ext = os.path.splitext(mission_filename)[1] if mission_filename_ext == '.plan': for waypoint in read_plan_file(f): wps.append(waypoint) rospy.logdebug(waypoint) elif mission_filename_ext == '.mission': for waypoint in read_mission_file(f): wps.append(waypoint) rospy.logdebug(waypoint) elif mission_filename_ext == '.txt': mission = QGroundControlWP() for waypoint in mission.read(f): wps.append(waypoint) rospy.logdebug(waypoint) else: raise IOError("unknown mission file extension", mission_filename_ext) # set first item to current if wps[0]: wps[0].is_current = True return wps
def upload_missions(self): actualMissionFile = '/home/anne/catkin_ws/src/agrodrone/agrodrone/tests/integration/' + self.filename wpl = [] if self.vehicle.fcu_type == "PX4": with open(actualMissionFile) as mission_file: data = json.load(mission_file) for item in data["items"]: waypoint = Waypoint() waypoint.command = item["command"] waypoint.autocontinue = item["autoContinue"] waypoint.frame = item["frame"] waypoint.x_lat = item["coordinate"][0] waypoint.y_long = item["coordinate"][1] waypoint.z_alt = item["coordinate"][2] wpl.append(waypoint) elif self.vehicle.fcu_type == "Ardupilot": mission = QGroundControlWP() for waypoint in mission.read(open(actualMissionFile, 'r')): wpl.append(waypoint) else: rospy.logerr("Unknown fcu type set") res = self._srv_wp_push(wpl) return res.success
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) mission = QGroundControlWP() wps = [] for waypoint in mission.read(open(mission_file, 'r')): wps.append(waypoint) rospy.logdebug(waypoint) 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
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) mission = QGroundControlWP() wps = [] for waypoint in mission.read(open(mission_file, 'r')): wps.append(waypoint) rospy.logdebug(waypoint) 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
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__)) + "/" + 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__)) + "/" + 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))