diff options
author | Andreas Antener <antener_a@gmx.ch> | 2015-03-15 12:07:35 +0100 |
---|---|---|
committer | Andreas Antener <antener_a@gmx.ch> | 2015-03-15 12:48:51 +0100 |
commit | 7018ff298ea3174d6764a1e35196521ee31f4dd3 (patch) | |
tree | 939b57fb28cfe020a749d096d68cdabb62c6ca68 /integrationtests | |
parent | 2bbad206961ec4bf1e78ac8b6689634fa3e482b8 (diff) | |
download | px4-firmware-7018ff298ea3174d6764a1e35196521ee31f4dd3.tar.gz px4-firmware-7018ff298ea3174d6764a1e35196521ee31f4dd3.tar.bz2 px4-firmware-7018ff298ea3174d6764a1e35196521ee31f4dd3.zip |
removing manual input and unused code from posctl test
Diffstat (limited to 'integrationtests')
-rwxr-xr-x | integrationtests/demo_tests/mavros_offboard_posctl_test.py | 71 |
1 files changed, 29 insertions, 42 deletions
diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 6d26015e7..568c2cbd8 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy import math @@ -49,9 +48,6 @@ from px4.msg import vehicle_control_mode from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput # # Tests flying a path in offboard control by sending position setpoints @@ -64,37 +60,41 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('iris/mavros/cmd/arming', 30) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) + self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.local_position = PoseStamped() + self.control_mode = vehicle_control_mode() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - if(not self.hasPos): + if not self.has_pos: return False - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + rospy.logdebug("current position %f, %f, %f" % + (self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) + desired = np.array((x, y, z)) - pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + pos = np.array((self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -114,57 +114,44 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 - while(count < timeout): + while count < timeout: # update timestamp for each published SP pos.header.stamp = rospy.Time.now() - self.pubSpt.publish(pos) + self.pub_spt.publish(pos) - if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to get to position") - def arm(self): - return self.cmdArm(value=True) - # # Test offboard position control # def test_posctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_posctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # prepare flight path positions = ( - (0,0,0), - (2,2,2), - (2,-2,2), - (-2,-2,2), - (2,2,2)) + (0, 0, 0), + (2, 2, 2), + (2, -2, 2), + (-2, -2, 2), + (2, 2, 2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - # does it hold the position for Y seconds? - positionHeld = True count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, 2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, 2, 0.5): break count = count + 1 self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count == timeout, "position could not be held") |