From 00d56b9ef8138889feeb7f6ff51ff38d52bb0a88 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 25 Apr 2016 21:04:22 +0200 Subject: [PATCH] added VTOL mission test, updated mission test to check mission depending on vehicle state --- .../python_src/px4_it/mavros/mission_test.py | 29 ++++++++++++++----- .../python_src/px4_it/mavros/vtol_mis_1.txt | 9 ++++++ .../mavros_posix_tests_standard_vtol.launch | 18 ++++++++++++ .../SITL/init/rcS_gazebo_standard_vtol | 10 ++++++- 4 files changed, 58 insertions(+), 8 deletions(-) create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt create mode 100644 launch/mavros_posix_tests_standard_vtol.launch diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 17124a65ca..28b47163cd 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -42,6 +42,7 @@ import rospy import math import rosbag import sys +import os import mavros from pymavlink import mavutil @@ -49,7 +50,7 @@ from mavros import mavlink from geometry_msgs.msg import PoseStamped from mavros_msgs.srv import CommandLong, WaypointPush -from mavros_msgs.msg import Mavlink, Waypoint +from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState from sensor_msgs.msg import NavSatFix from mavros.mission import QGroundControlWP #from px4_test_helper import PX4TestHelper @@ -66,6 +67,7 @@ class MavrosMissionTest(unittest.TestCase): rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback) rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback) + rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback) self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) rospy.wait_for_service('mavros/cmd/command', 30) self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True) @@ -74,7 +76,10 @@ class MavrosMissionTest(unittest.TestCase): self.has_global_pos = False self.local_position = PoseStamped() self.global_position = NavSatFix() + self.extended_state = ExtendedState() self.home_alt = 0 + self.mc_rad = 5 + self.fw_rad = 40 # need to simulate heartbeat for datalink loss detection rospy.Timer(rospy.Duration(0.5), self.send_heartbeat) @@ -96,6 +101,9 @@ class MavrosMissionTest(unittest.TestCase): self.home_alt = data.altitude self.has_global_pos = True + def extended_state_callback(self, data): + self.extended_state = data + # # Helper methods # @@ -115,13 +123,18 @@ class MavrosMissionTest(unittest.TestCase): d = R * c alt_d = abs(alt - self.global_position.altitude) - rospy.loginfo("d: %f, alt_d: %f", d, alt_d) + #rospy.loginfo("d: %f, alt_d: %f", d, alt_d) return d < offset and alt_d < offset - def reach_position(self, lat, lon, alt, radius, timeout): + def reach_position(self, lat, lon, alt, timeout): # does it reach the position in X seconds? count = 0 while count < timeout: + # use different radius matching vehicle state + radius = self.mc_rad + if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW: + radius = self.fw_rad + if self.is_at_position(lat, lon, alt, radius): break count = count + 1 @@ -153,13 +166,15 @@ class MavrosMissionTest(unittest.TestCase): """Test mission""" if len(sys.argv) < 2: - self.fail("no mission argument") + self.fail("usage: mission_test.py mission_file") return - rospy.loginfo("reading mission") + file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1] + + rospy.loginfo("reading mission %s", file) mission = QGroundControlWP() wps = [] - for wp in mission.read(open(sys.argv[1], 'r')): + for wp in mission.read(open(file, 'r')): wps.append(wp) rospy.logdebug(wp) @@ -187,7 +202,7 @@ class MavrosMissionTest(unittest.TestCase): alt = wp.z_alt if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT: alt += self.home_alt - self.reach_position(wp.x_lat, wp.y_long, alt, 10, 300) + self.reach_position(wp.x_lat, wp.y_long, alt, 600) if __name__ == '__main__': diff --git a/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt new file mode 100644 index 0000000000..6d61dbfc96 --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt @@ -0,0 +1,9 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 47.3979949951 8.54559612274 12.0 1 +1 0 2 3000 4.0 0.0 0.0 0.0 47.3980331421 8.54578971863 12.0 1 +2 0 3 16 0.0 0.0 -0.0 0.0 47.399269104 8.54557228088 12.0 1 +3 0 3 16 0.0 0.0 -0.0 0.0 47.3992652893 8.54230213165 12.0 1 +4 0 3 16 0.0 0.0 -0.0 0.0 47.3974761963 8.54239082336 12.0 1 +5 0 3 16 0.0 0.0 -0.0 0.0 47.3976669312 8.54509449005 12.0 1 +6 0 2 3000 3.0 0.0 0.0 0.0 47.3977851868 8.54526233673 12.0 1 +7 0 3 21 25.0 0.0 -0.0 0.0 47.3979797363 8.54460906982 12.0 1 diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch new file mode 100644 index 0000000000..58b928995d --- /dev/null +++ b/launch/mavros_posix_tests_standard_vtol.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol index f43d6330b4..44294fa203 100644 --- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol +++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol @@ -39,7 +39,15 @@ param set NAV_DLL_ACT 2 param set NAV_ACC_RAD 3.0 param set MPC_TKO_SPEED 1.0 param set MIS_YAW_TMT 10 -simulator start -s +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set RTL_LAND_DELAY 0 +param set COM_DISARM_LAND 5 +param set COM_DL_LOSS_EN 1 +param set MPC_ACC_HOR_MAX 2 +param set GF_ACTION 3 +param set GF_MAX_HOR_DIST 500 +param set GF_MAX_VER_DIST 500 rgbledsim start tone_alarm start gyrosim start