From e0e7f8c5177b813b3e6e8eb0477f9ee32c06c407 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 18:02:43 +0100 Subject: - updated manual input to publish directly to px4 and not over joystick topics - updated tests to work with current setup --- integrationtests/demo_tests/demo_tests.launch | 9 +- integrationtests/demo_tests/manual_input.py | 95 ++++++++++++++++----- .../demo_tests/offboard_posctl_test.py | 96 ++++++++++++++++++++++ integrationtests/demo_tests/posctl_test.py | 96 ---------------------- 4 files changed, 174 insertions(+), 122 deletions(-) create mode 100755 integrationtests/demo_tests/offboard_posctl_test.py delete mode 100755 integrationtests/demo_tests/posctl_test.py (limited to 'integrationtests') diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch index b5377659f..34281b781 100644 --- a/integrationtests/demo_tests/demo_tests.launch +++ b/integrationtests/demo_tests/demo_tests.launch @@ -1,8 +1,8 @@ - - + + - + @@ -12,7 +12,8 @@ + - + diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 4bf508509..eb9144bbc 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -2,43 +2,59 @@ import sys import rospy -from sensor_msgs.msg import Joy +from px4.msg import manual_control_setpoint +from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header - # -# Manual input control helper, fakes joystick input -# > needs to correspond to default mapping in manual_input node +# Manual input control helper +# +# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else +# the simulator does not instantiate our controller. # class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) - self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10) - self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10) + self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + att = CommandAttitudeThrust() + att.header = Header() + + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("zeroing") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) rate.sleep() count = count + 1 - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos.r = 1 count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("arming") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 @@ -46,14 +62,49 @@ class ManualInput: rate = rospy.Rate(10) # 10hz # triggers posctl - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 1, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + + def offboard(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("triggering posctl") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 + diff --git a/integrationtests/demo_tests/offboard_posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py new file mode 100755 index 000000000..0d5608245 --- /dev/null +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_control_mode +from px4.msg import actuator_armed +from px4.msg import position_setpoint_triplet +from px4.msg import position_setpoint +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + +from manual_input import ManualInput + + +class OffboardPosctlTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) + + # + # Test offboard POSCTL + # + def test_posctl(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rate = rospy.Rate(10) # 10hz + + manIn = ManualInput() + + # arm and go into offboard + manIn.arm() + manIn.offboard() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = 2 + pos.z = -2 + pos.y = 2 + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + timeout = 120 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + positionHeld = False + break + count = count + 1 + rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/posctl_test.py deleted file mode 100755 index 1cbf09cf7..000000000 --- a/integrationtests/demo_tests/posctl_test.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/env python -PKG = 'px4' - -import sys -import unittest -import rospy - -from px4.msg import vehicle_local_position -from px4.msg import vehicle_control_mode -from px4.msg import actuator_armed -from px4.msg import position_setpoint_triplet -from px4.msg import position_setpoint -from sensor_msgs.msg import Joy -from std_msgs.msg import Header - -from manual_input import ManualInput - - -class PosctlTest(unittest.TestCase): - - # - # General callback functions used in tests - # - def position_callback(self, data): - self.hasPos = True - self.localPosition = data - - def vehicle_control_mode_callback(self, data): - self.controlMode = data - - - # - # Helper methods - # - def is_at_position(self, x, y, z, offset): - rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) - - # - # Test POSCTL - # - def test_posctl(self): - rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) - rate = rospy.Rate(10) # 10hz - - manIn = ManualInput() - - # arm and go into POSCTL - manIn.arm() - manIn.posctl() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - - # set a position setpoint - pos = position_setpoint() - pos.valid = True - pos.x = 2 - pos.z = -2 - pos.y = 2 - pos.position_valid = True - stp = position_setpoint_triplet() - stp.current = pos - pubSpt.publish(stp) - - # does it reach the position in X seconds? - count = 0 - timeout = 120 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): - break - count = count + 1 - rate.sleep() - - self.assertTrue(count < timeout, "took too long to get to position") - - # does it hold the position for Y seconds? - positionHeld = True - count = 0 - timeout = 50 - while(count < timeout): - if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): - positionHeld = False - break - count = count + 1 - rate.sleep() - - self.assertTrue(count == timeout, "position could not be held") - - -if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'posctl_test', PosctlTest) -- cgit v1.2.3