diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index ec2b1bc5ae..36077f32dd 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -48,13 +48,13 @@ from tf.transformations import quaternion_from_euler from mavros_msgs.srv import CommandLong #from px4_test_helper import PX4TestHelper -# -# Tests flying a path in offboard control by sending attitude and thrust setpoints -# over MAVROS. -# -# For the test to be successful it needs to cross a certain boundary in time. -# class MavrosOffboardAttctlTest(unittest.TestCase): + """ + Tests flying in offboard control by sending attitude and thrust setpoints + via MAVROS. + + For the test to be successful it needs to cross a certain boundary in time. + """ def setUp(self): rospy.init_node('test_node', anonymous=True) @@ -82,10 +82,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.has_pos = True self.local_position = data - # - # Test offboard position control - # def test_attctl(self): + """Test offboard attitude control""" + # set some attitude and thrust att = PoseStamped() att.header = Header() @@ -95,7 +94,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.pose.orientation = Quaternion(*quaternion) throttle = Float64() - throttle.data = 0.6 + throttle.data = 0.8 armed = False # does it cross expected boundaries in X seconds? @@ -112,7 +111,8 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.rate.sleep() # arm and switch to offboard - if not armed: + # (need to wait the first few rounds until PX4 has the offboard stream) + if not armed and count > 5: self._srv_cmd_long(False, 176, False, 128 | 1, 6, 0, 0, 0, 0, 0) armed = True diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 8d6e9cd8bc..f42617cfd8 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -48,16 +48,17 @@ import numpy as np from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from mavros_msgs.srv import CommandLong #from px4_test_helper import PX4TestHelper -# -# 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): + """ + Tests flying a path in offboard control by sending position setpoints + via 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) + """ def setUp(self): rospy.init_node('test_node', anonymous=True) @@ -66,9 +67,12 @@ class MavrosOffboardPosctlTest(unittest.TestCase): rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback) self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + rospy.wait_for_service('mavros/cmd/command', 30) + self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True) self.rate = rospy.Rate(10) # 10hz self.has_pos = False self.local_position = PoseStamped() + self.armed = False def tearDown(self): #self.helper.tearDown() @@ -90,9 +94,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): return False 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)) + (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.local_position.pose.position.x, @@ -123,6 +127,13 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.pub_spt.publish(pos) #self.helper.bag_write('mavros/setpoint_position/local', pos) + # arm and switch to offboard + # (need to wait the first few rounds until PX4 has the offboard stream) + if not self.armed and count > 5: + self._srv_cmd_long(False, 176, False, + 128 | 1, 6, 0, 0, 0, 0, 0) + self.armed = True + if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break count = count + 1 @@ -130,11 +141,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.assertTrue(count < timeout, "took too long to get to position") - # - # Test offboard position control - # def test_posctl(self): - # prepare flight path + """Test offboard position control""" + positions = ( (0, 0, 0), (2, 2, 2), @@ -143,7 +152,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): (2, 2, 2)) 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], 180) count = 0 timeout = 50 diff --git a/integrationtests/demo_tests/mavros_tests_posix.launch b/integrationtests/demo_tests/mavros_tests_posix.launch index 1f89bbfb03..029ef81590 100644 --- a/integrationtests/demo_tests/mavros_tests_posix.launch +++ b/integrationtests/demo_tests/mavros_tests_posix.launch @@ -20,7 +20,7 @@ - + diff --git a/integrationtests/run-tests.bash b/integrationtests/run-tests.bash index e3b916b4c9..139b292150 100755 --- a/integrationtests/run-tests.bash +++ b/integrationtests/run-tests.bash @@ -6,8 +6,11 @@ set -e SRC_DIR=/root/Firmware +BUILD=posix_sitl_default # TODO -TEST_RESULT_DIR=/root/.ros/test_results/px4 +ROS_TEST_RESULT_DIR=/root/.ros/test_results/px4 +ROS_LOG_DIR=/root/.ros/log +PX4_LOG_DIR=${SRC_DIR}/build_${BUILD}/src/firmware/posix/rootfs/fs/microsd/log TEST_RESULT_TARGET_DIR=/job/test_results # BAGS=/root/.ros # CHARTS=/root/.ros/charts @@ -33,7 +36,7 @@ ln -s /job/Firmware ${SRC_DIR} echo "=====> compile" cd $SRC_DIR -make posix_sitl_default +make ${BUILD} mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build cmake -Wno-dev .. @@ -58,7 +61,9 @@ echo "=====> process test results" echo "copy build test results to job directory" mkdir -p ${TEST_RESULT_TARGET_DIR} -cp -r $TEST_RESULT_DIR/* ${TEST_RESULT_TARGET_DIR} +cp -r $ROS_TEST_RESULT_DIR/* ${TEST_RESULT_TARGET_DIR} +cp -r $ROS_LOG_DIR/* ${TEST_RESULT_TARGET_DIR} +cp -r $PX4_LOG_DIR/* ${TEST_RESULT_TARGET_DIR} # cp $BAGS/*.bag ${TEST_RESULT_TARGET_DIR}/ # cp -r $CHARTS ${TEST_RESULT_TARGET_DIR}/ echo "<====="