diff options
Diffstat (limited to 'integrationtests/demo_tests/mavros_offboard_posctl_test.py')
| -rwxr-xr-x | integrationtests/demo_tests/mavros_offboard_posctl_test.py | 42 |
1 files changed, 35 insertions, 7 deletions
diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 7468ad53f..a1f1cf3c5 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -49,16 +49,30 @@ 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 -class OffboardPosctlTest(unittest.TestCase): +from manual_input import ManualInput + +# +# Tests flying a path in offboard control by sending position setpoints +# over MAVROS. +# +# For the test to be successful it needs to reach all setpoints in a certain time. +# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# +class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + rospy.wait_for_service('mavros/cmd/arming', 30) rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10) + rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz + self.rateSec = rospy.Rate(1) self.hasPos = False + self.controlMode = vehicle_control_mode() # # General callback functions used in tests @@ -88,7 +102,6 @@ class OffboardPosctlTest(unittest.TestCase): pos = PoseStamped() pos.header = Header() pos.header.frame_id = "base_footprint" - pos.header.stamp = rospy.Time.now() pos.pose.position.x = x pos.pose.position.y = y pos.pose.position.z = z @@ -102,6 +115,8 @@ class OffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 while(count < timeout): + # update timestamp for each published SP + pos.header.stamp = rospy.Time.now() self.pubSpt.publish(pos) if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): @@ -111,11 +126,24 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(count < timeout, "took too long to get to position") + def arm(self): + return self.cmdArm(value=True) + # - # Test offboard POSCTL + # Test offboard position control # def test_posctl(self): - # prepare flight path assertion + # FIXME: this must go ASAP when arming is implemented + manIn = ManualInput() + manIn.arm() + manIn.offboard() + + 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), @@ -142,5 +170,5 @@ class OffboardPosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest) #unittest.main() |