Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 19:57:12 +08:00
Code Issues Actions 14 Packages Projects Releases Wiki Activity

removing manual input and unused code from posctl test

Browse Source
This commit is contained in:
Andreas Antener
2015-03-15 12:07:35 +01:00
parent 2bbad20696
commit 7018ff298e
1 changed files with 29 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
integrationtests/demo_tests/mavros_offboard_posctl_test.py
+29 -42
View File
@@ -37,7 +37,6 @@
# #
PKG = 'px4' PKG = 'px4'
import sys
import unittest import unittest
import rospy import rospy
import math import math
@@ -49,9 +48,6 @@ from px4.msg import vehicle_control_mode
from std_msgs.msg import Header from std_msgs.msg import Header
from geometry_msgs.msg import PoseStamped, Quaternion from geometry_msgs.msg import PoseStamped, Quaternion
from tf.transformations import quaternion_from_euler 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 # Tests flying a path in offboard control by sending position setpoints
@@ -64,37 +60,41 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) 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/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_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.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10)
self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool)
self.rate = rospy.Rate(10) # 10hz self.rate = rospy.Rate(10) # 10hz
self.rateSec = rospy.Rate(1) self.has_pos = False
self.hasPos = False self.local_position = PoseStamped()
self.controlMode = vehicle_control_mode() self.control_mode = vehicle_control_mode()
# #
# General callback functions used in tests # General callback functions used in tests
# #
def position_callback(self, data): def position_callback(self, data):
self.hasPos = True self.has_pos = True
self.localPosition = data self.local_position = data
def vehicle_control_mode_callback(self, data): def vehicle_control_mode_callback(self, data):
self.controlMode = data self.control_mode = data
# #
# Helper methods # Helper methods
# #
def is_at_position(self, x, y, z, offset): def is_at_position(self, x, y, z, offset):
if(not self.hasPos): if not self.has_pos:
return False 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)) 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 return linalg.norm(desired - pos) < offset
def reach_position(self, x, y, z, timeout): def reach_position(self, x, y, z, timeout):
@@ -114,57 +114,44 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
# does it reach the position in X seconds? # does it reach the position in X seconds?
count = 0 count = 0
while(count < timeout): while count < timeout:
# update timestamp for each published SP # update timestamp for each published SP
pos.header.stamp = rospy.Time.now() 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 break
count = count + 1 count = count + 1
self.rate.sleep() self.rate.sleep()
self.assertTrue(count < timeout, "took too long to get to position") self.assertTrue(count < timeout, "took too long to get to position")
def arm(self):
return self.cmdArm(value=True)
# #
# Test offboard position control # Test offboard position control
# #
def test_posctl(self): 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 # prepare flight path
positions = ( positions = (
(0,0,0), (0, 0, 0),
(2,2,2), (2, 2, 2),
(2,-2,2), (2, -2, 2),
(-2,-2,2), (-2, -2, 2),
(2,2,2)) (2, 2, 2))
for i in range(0, len(positions)): for i in range(0, len(positions)):
self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) 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 count = 0
timeout = 50 timeout = 50
while(count < timeout): while count < timeout:
if(not self.is_at_position(2, 2, 2, 0.5)): if not self.is_at_position(2, 2, 2, 0.5):
positionHeld = False
break break
count = count + 1 count = count + 1
self.rate.sleep() 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") self.assertTrue(count == timeout, "position could not be held")