update sitl tests

integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py

@@ -42,16 +42,13 @@ PKG = 'px4'
 
 import unittest
 import rospy
-import rosbag
-import time
-
-from std_msgs.msg import Header
-from std_msgs.msg import Float64
-from geometry_msgs.msg import PoseStamped, Quaternion
 from tf.transformations import quaternion_from_euler
-from mavros_msgs.srv import CommandLong
+from geometry_msgs.msg import PoseStamped, Quaternion, Vector3
+from mavros_msgs.msg import AttitudeTarget, State
+from mavros_msgs.srv import CommandBool, SetMode
 from sensor_msgs.msg import NavSatFix
-#from px4_test_helper import PX4TestHelper
+from std_msgs.msg import Header
+
 
 class MavrosOffboardAttctlTest(unittest.TestCase):
     """
@@ -62,23 +59,26 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
     """
 
     def setUp(self):
-        rospy.init_node('test_node', anonymous=True)
+        self.rate = rospy.Rate(10)  # 10hz
-        rospy.wait_for_service('mavros/cmd/arming', 30)
-        #self.helper = PX4TestHelper("mavros_offboard_attctl_test")
-        #self.helper.setUp()
-
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
-        self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10)
-        self.pub_thr = rospy.Publisher('mavros/setpoint_attitude/att_throttle', Float64, 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_global_pos = False
         self.local_position = PoseStamped()
+        self.state = State()
+
+        # setup ROS topics and services
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.wait_for_service('mavros/set_mode', 30)
+        self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
+                                                 CommandBool)
+        self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
+        rospy.Subscriber('mavros/local_position/pose', PoseStamped,
+                         self.position_callback)
+        rospy.Subscriber('mavros/global_position/global', NavSatFix,
+                         self.global_position_callback)
+        rospy.Subscriber('mavros/state', State, self.state_callback)
+        self.att_setpoint_pub = rospy.Publisher(
+            'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)
 
     def tearDown(self):
-        #self.helper.tearDown()
         pass
 
     #
@@ -90,62 +90,90 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
     def global_position_callback(self, data):
         self.has_global_pos = True
 
-    def test_attctl(self):
+    def state_callback(self, data):
-        """Test offboard attitude control"""
+        self.state = data
 
-        # FIXME: hack to wait for simulation to be ready
+    #
+    # Helper methods
+    #
+    def wait_until_ready(self):
+        """FIXME: hack to wait for simulation to be ready"""
+        rospy.loginfo("waiting for global position")
         while not self.has_global_pos:
             self.rate.sleep()
 
+    #
+    # Test method
+    #
+    def test_attctl(self):
+        """Test offboard attitude control"""
+
+        self.wait_until_ready()
+
         # set some attitude and thrust
-        att = PoseStamped()
+        att = AttitudeTarget()
         att.header = Header()
         att.header.frame_id = "base_footprint"
         att.header.stamp = rospy.Time.now()
-        quaternion = quaternion_from_euler(0.25, 0.25, 0)
+        att.orientation = Quaternion(*quaternion_from_euler(0.25, 0.25, 0))
-        att.pose.orientation = Quaternion(*quaternion)
+        att.body_rate = Vector3()
+        att.thrust = 0.7
+        att.type_mask = 7
 
-        throttle = Float64()
+        # send some setpoints before starting
-        throttle.data = 0.7
+        for i in xrange(20):
-        armed = False
-
-        # does it cross expected boundaries in X seconds?
-        count = 0
-        timeout = 120
-        while count < timeout:
-            # update timestamp for each published SP
             att.header.stamp = rospy.Time.now()
-
+            self.att_setpoint_pub.publish(att)
-            self.pub_att.publish(att)
-            #self.helper.bag_write('mavros/setpoint_attitude/attitude', att)
-            self.pub_thr.publish(throttle)
-            #self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle)
             self.rate.sleep()
 
-            # FIXME: arm and switch to offboard
+        rospy.loginfo("set mission mode and arm")
-            # (need to wait the first few rounds until PX4 has the offboard stream)
+        while self.state.mode != "OFFBOARD" or not self.state.armed:
-            if not armed and count > 5:
+            if self.state.mode != "OFFBOARD":
-                self._srv_cmd_long(False, 176, False,
+                try:
-                                   1, 6, 0, 0, 0, 0, 0)
+                    self.set_mode_srv(0, 'OFFBOARD')
-                # make sure the first command doesn't get lost
+                except rospy.ServiceException as e:
-                time.sleep(1)
+                    rospy.logerr(e)
+            if not self.state.armed:
+                try:
+                    self.set_arming_srv(True)
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+            rospy.sleep(2)
 
-                self._srv_cmd_long(False, 400, False,
+        rospy.loginfo("run mission")
-                                   # arm
+        # does it cross expected boundaries in X seconds?
-                                   1, 0, 0, 0, 0, 0, 0)
+        timeout = 120
+        crossed = False
+        for count in xrange(timeout):
+            # update timestamp for each published SP
+            att.header.stamp = rospy.Time.now()
+            self.att_setpoint_pub.publish(att)
 
-                armed = True
+            if (self.local_position.pose.position.x > 5 and
-
+                    self.local_position.pose.position.z > 5 and
-            if (self.local_position.pose.position.x > 5
+                    self.local_position.pose.position.y < -5):
-                    and self.local_position.pose.position.z > 5
+                rospy.loginfo("boundary crossed | count {0}".format(count))
-                    and self.local_position.pose.position.y < -5):
+                crossed = True
                 break
-            count = count + 1
 
-        self.assertTrue(count < timeout, "took too long to cross boundaries")
+            self.rate.sleep()
+
+        self.assertTrue(crossed, (
+            "took too long to cross boundaries | x: {0}, y: {1}, z: {2}, timeout: {3}".
+            format(self.local_position.pose.position.x,
+                   self.local_position.pose.position.y,
+                   self.local_position.pose.position.z, timeout)))
+
+        if self.state.armed:
+            try:
+                self.set_arming_srv(False)
+            except rospy.ServiceException as e:
+                rospy.logerr(e)
 
 
 if __name__ == '__main__':
     import rostest
-    rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest)
+    rospy.init_node('test_node', anonymous=True)
-    #unittest.main()
+
+    rostest.rosrun(PKG, 'mavros_offboard_attctl_test',
+                   MavrosOffboardAttctlTest)

integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py

@@ -43,18 +43,14 @@ PKG = 'px4'
 import unittest
 import rospy
 import math
-import rosbag
-import time
-
-from numpy import linalg
 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 geometry_msgs.msg import PoseStamped, Quaternion
+from mavros_msgs.msg import State
+from mavros_msgs.srv import CommandBool, SetMode
 from sensor_msgs.msg import NavSatFix
-#from px4_test_helper import PX4TestHelper
+from std_msgs.msg import Header
+
 
 class MavrosOffboardPosctlTest(unittest.TestCase):
     """
@@ -66,22 +62,26 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
     """
 
     def setUp(self):
-        rospy.init_node('test_node', anonymous=True)
+        self.rate = rospy.Rate(10)  # 10hz
-        #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
-        #self.helper.setUp()
-
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_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_global_pos = False
         self.local_position = PoseStamped()
-        self.armed = False
+        self.state = State()
+
+        # setup ROS topics and services
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.wait_for_service('mavros/set_mode', 30)
+        self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
+                                                 CommandBool)
+        self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+        rospy.Subscriber('mavros/local_position/pose', PoseStamped,
+                         self.position_callback)
+        rospy.Subscriber('mavros/global_position/global', NavSatFix,
+                         self.global_position_callback)
+        rospy.Subscriber('mavros/state', State, self.state_callback)
+        self.pos_setpoint_pub = rospy.Publisher(
+            'mavros/setpoint_position/local', PoseStamped, queue_size=10)
 
     def tearDown(self):
-        #self.helper.tearDown()
         pass
 
     #
@@ -93,20 +93,28 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
     def global_position_callback(self, data):
         self.has_global_pos = True
 
+    def state_callback(self, data):
+        self.state = data
+
     #
     # Helper methods
     #
+    def wait_until_ready(self):
+        """FIXME: hack to wait for simulation to be ready"""
+        rospy.loginfo("waiting for global position")
+        while not self.has_global_pos:
+            self.rate.sleep()
+
     def is_at_position(self, x, y, z, offset):
-        rospy.logdebug("current position %f, %f, %f" %
+        rospy.logdebug("current position | x:{0}, y:{1}, z:{2}".format(
-                       (self.local_position.pose.position.x,
+            self.local_position.pose.position.x, self.local_position.pose.
-                       self.local_position.pose.position.y,
+            position.y, self.local_position.pose.position.z))
-                       self.local_position.pose.position.z))
 
         desired = np.array((x, y, 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 np.linalg.norm(desired - pos) < offset
 
     def reach_position(self, x, y, z, timeout):
         # set a position setpoint
@@ -123,54 +131,75 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
         quaternion = quaternion_from_euler(0, 0, yaw)
         pos.pose.orientation = Quaternion(*quaternion)
 
-        # does it reach the position in X seconds?
+        # send some setpoints before starting
-        count = 0
+        for i in xrange(20):
-        while count < timeout:
-            # update timestamp for each published SP
             pos.header.stamp = rospy.Time.now()
-            self.pub_spt.publish(pos)
+            self.pos_setpoint_pub.publish(pos)
-            #self.helper.bag_write('mavros/setpoint_position/local', pos)
-
-            # FIXME: 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,
-                                   1, 6, 0, 0, 0, 0, 0)
-                # make sure the first command doesn't get lost
-                time.sleep(1)
-
-                self._srv_cmd_long(False, 400, False,
-                                   # arm
-                                   1, 0, 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, 1):
-                break
-            count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, "took too long to get to position")
+        rospy.loginfo("set mission mode and arm")
+        while self.state.mode != "OFFBOARD" or not self.state.armed:
+            if self.state.mode != "OFFBOARD":
+                try:
+                    self.set_mode_srv(0, 'OFFBOARD')
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+            if not self.state.armed:
+                try:
+                    self.set_arming_srv(True)
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+            rospy.sleep(2)
 
+        rospy.loginfo("run mission")
+        # does it reach the position in X seconds?
+        reached = False
+        for count in xrange(timeout):
+            # update timestamp for each published SP
+            pos.header.stamp = rospy.Time.now()
+            self.pos_setpoint_pub.publish(pos)
+
+            if self.is_at_position(pos.pose.position.x, pos.pose.position.y,
+                                   pos.pose.position.z, 1):
+                rospy.loginfo(
+                    "position reached | count: {0}, x: {0}, y: {1}, z: {2}".
+                    format(count, pos.pose.position.x, pos.pose.position.y,
+                           pos.pose.position.z))
+                reached = True
+                break
+
+            self.rate.sleep()
+
+        self.assertTrue(reached, (
+            "took too long to get to position | x: {0}, y: {1}, z: {2}, timeout: {3}".
+            format(self.local_position.pose.position.x,
+                   self.local_position.pose.position.y,
+                   self.local_position.pose.position.z, timeout)))
+
+    #
+    # Test method
+    #
     def test_posctl(self):
         """Test offboard position control"""
 
-        # FIXME: hack to wait for simulation to be ready
+        self.wait_until_ready()
-        while not self.has_global_pos:
-            self.rate.sleep()
 
-        positions = (
+        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)):
+        for i in xrange(len(positions)):
-            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 180)
+            self.reach_position(positions[i][0], positions[i][1],
+                                positions[i][2], 180)
+
+        if self.state.armed:
+            try:
+                self.set_arming_srv(False)
+            except rospy.ServiceException as e:
+                rospy.logerr(e)
 
 
 if __name__ == '__main__':
     import rostest
-    rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest)
+    rospy.init_node('test_node', anonymous=True)
-    #unittest.main()
+
+    rostest.rosrun(PKG, 'mavros_offboard_posctl_test',
+                   MavrosOffboardPosctlTest)

integrationtests/python_src/px4_it/mavros/mission_test.py

@@ -43,91 +43,102 @@ PKG = 'px4'
 
 import unittest
 import rospy
-import math
-import rosbag
-import sys
-import os
-import time
 import glob
 import json
-
+import math
-import mavros
+import os
-from pymavlink import mavutil
-from mavros import mavlink
-
 import px4tools
-
+import sys
-from geometry_msgs.msg import PoseStamped
+from mavros import mavlink
-from mavros_msgs.srv import CommandLong, WaypointPush
-from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
-from sensor_msgs.msg import NavSatFix
 from mavros.mission import QGroundControlWP
-#from px4_test_helper import PX4TestHelper
+from pymavlink import mavutil
+from threading import Thread
+from geometry_msgs.msg import PoseStamped
+from mavros_msgs.msg import Altitude, ExtendedState, Mavlink, State, Waypoint
+from mavros_msgs.srv import CommandBool, SetMode, WaypointPush
+from sensor_msgs.msg import NavSatFix
+
 
 def get_last_log():
     try:
         log_path = os.environ['PX4_LOG_DIR']
     except KeyError:
-        log_path = os.path.join(os.environ['HOME'], 'ros/rootfs/fs/microsd/log')
+        log_path = os.path.join(os.environ['HOME'],
-    last_log_dir = sorted(
+                                '.ros/rootfs/fs/microsd/log')
-        glob.glob(os.path.join(log_path, '*')))[-1]
+    last_log_dir = sorted(glob.glob(os.path.join(log_path, '*')))[-1]
     last_log = sorted(glob.glob(os.path.join(last_log_dir, '*.ulg')))[-1]
     return last_log
 
+
 def read_new_mission(f):
     d = json.load(f)
     current = True
     for wp in d['items']:
         yield Waypoint(
-                is_current = current,
+            is_current=current,
-                frame = int(wp['frame']),
+            frame=int(wp['frame']),
-                command = int(wp['command']),
+            command=int(wp['command']),
-                param1 = float(wp['param1']),
+            param1=float(wp['param1']),
-                param2 = float(wp['param2']),
+            param2=float(wp['param2']),
-                param3 = float(wp['param3']),
+            param3=float(wp['param3']),
-                param4 = float(wp['param4']),
+            param4=float(wp['param4']),
-                x_lat = float(wp['coordinate'][0]),
+            x_lat=float(wp['coordinate'][0]),
-                y_long = float(wp['coordinate'][1]),
+            y_long=float(wp['coordinate'][1]),
-                z_alt = float(wp['coordinate'][2]),
+            z_alt=float(wp['coordinate'][2]),
-                autocontinue = bool(wp['autoContinue']))
+            autocontinue=bool(wp['autoContinue']))
         if current:
             current = False
 
+
 class MavrosMissionTest(unittest.TestCase):
     """
     Run a mission
     """
 
     def setUp(self):
-        rospy.init_node('test_node', anonymous=True)
+        self.rate = rospy.Rate(10)  # 10hz
-
-        self.rate = rospy.Rate(10) # 10hz
         self.has_global_pos = False
         self.local_position = PoseStamped()
         self.global_position = NavSatFix()
         self.extended_state = ExtendedState()
-        self.home_alt = 0
+        self.altitude = Altitude()
+        self.state = State()
         self.mc_rad = 5
         self.fw_rad = 60
         self.fw_alt_rad = 10
-        self.last_alt_d = 9999
+        self.last_alt_d = None
-        self.last_pos_d = 9999
+        self.last_pos_d = None
         self.mission_name = ""
 
-        # need to simulate heartbeat for datalink loss detection
+        # setup ROS topics and services
-        rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
+        rospy.wait_for_service('mavros/mission/push', 30)
+        rospy.wait_for_service('mavros/cmd/arming', 30)
+        rospy.wait_for_service('mavros/set_mode', 30)
+        self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
+                                              WaypointPush)
+        self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
+                                                 CommandBool)
+        self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+        rospy.Subscriber('mavros/local_position/pose', PoseStamped,
+                         self.position_callback)
+        rospy.Subscriber('mavros/global_position/global', NavSatFix,
+                         self.global_position_callback)
+        rospy.Subscriber('mavros/extended_state', ExtendedState,
+                         self.extended_state_callback)
+        rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback)
+        rospy.Subscriber('mavros/state', State, self.state_callback)
+        self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
 
-        rospy.wait_for_service('mavros/cmd/command', 30)
+        # need to simulate heartbeat to prevent datalink loss detection
-        self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
+        self.hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(
-        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
+            mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0)
-        self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True)
+        self.hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1))
-
+        self.hb_ros_msg = mavlink.convert_to_rosmsg(self.hb_mav_msg)
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
+        self.hb_thread = Thread(target=self.send_heartbeat, args=())
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
+        self.hb_thread.daemon = True
-        rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
+        self.hb_thread.start()
 
     def tearDown(self):
-        #self.helper.tearDown()
         pass
 
     #
@@ -140,77 +151,91 @@ class MavrosMissionTest(unittest.TestCase):
         self.global_position = data
 
         if not self.has_global_pos:
-            if data.altitude != 0:
+            self.has_global_pos = True
-                self.home_alt = data.altitude
-                self.has_global_pos = True
 
     def extended_state_callback(self, data):
-
+        prev_state = self.extended_state.vtol_state
-        prev_state = self.extended_state.vtol_state;
-
         self.extended_state = data
+
         if (prev_state != self.extended_state.vtol_state):
-            print("VTOL state change: %d" % self.extended_state.vtol_state);
+            rospy.loginfo("VTOL state change: {0}".format(
+                self.extended_state.vtol_state))
+
+    def state_callback(self, data):
+        self.state = data
+
+    def altitude_callback(self, data):
+        self.altitude = data
 
     #
     # Helper methods
     #
+    def send_heartbeat(self):
+        while not rospy.is_shutdown():
+            self.mavlink_pub.publish(self.hb_ros_msg)
+            try:
+                rospy.sleep(0.5)
+            except rospy.ROSInterruptException:
+                pass
+
     def is_at_position(self, lat, lon, alt, xy_offset, z_offset):
-        R = 6371000 # metres
+        R = 6371000  # metres
         rlat1 = math.radians(lat)
         rlat2 = math.radians(self.global_position.latitude)
 
         rlat_d = math.radians(self.global_position.latitude - lat)
         rlon_d = math.radians(self.global_position.longitude - lon)
 
-        a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) +
+        a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) + math.cos(rlat1) *
-             math.cos(rlat1) * math.cos(rlat2) *
+             math.cos(rlat2) * math.sin(rlon_d / 2) * math.sin(rlon_d / 2))
-             math.sin(rlon_d / 2) * math.sin(rlon_d / 2))
+        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
-        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
 
         d = R * c
-        alt_d = abs(alt - self.global_position.altitude)
+        alt_d = abs(alt - self.altitude.amsl)
-
-        #rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
 
         # remember best distances
-        if self.last_pos_d > d:
+        if not self.last_pos_d or self.last_pos_d > d:
             self.last_pos_d = d
-        if self.last_alt_d > alt_d:
+        if not self.last_alt_d or self.last_alt_d > alt_d:
             self.last_alt_d = alt_d
 
+        rospy.logdebug("d: {0}, alt_d: {1}".format(d, alt_d))
         return d < xy_offset and alt_d < z_offset
 
     def reach_position(self, lat, lon, alt, timeout, index):
         # reset best distances
-        self.last_alt_d = 9999
+        self.last_alt_d = None
-        self.last_pos_d = 9999
+        self.last_pos_d = None
 
-        rospy.loginfo("trying to reach waypoint " +
+        rospy.loginfo(
-            "lat: %13.9f, lon: %13.9f, alt: %6.2f, timeout: %d, index: %d" %
+            "trying to reach waypoint | " +
-            (lat, lon, alt, timeout, index))
+            "lat: {0:13.9f}, lon: {1:13.9f}, alt: {2:6.2f}, timeout: {3}, index: {4}".
+            format(lat, lon, alt, timeout, index))
 
         # does it reach the position in X seconds?
-        count = 0
+        reached = False
-        while count < timeout:
+        for count in xrange(timeout):
             # use MC radius by default
             # FIXME: also check MAV_TYPE from system status, otherwise pure fixed-wing won't work
             xy_radius = self.mc_rad
             z_radius = self.mc_rad
 
             # use FW radius if in FW or in transition
-            if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW or
+            if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC or
+                    or self.extended_state.vtol_state ==
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
+                    ExtendedState.VTOL_STATE_TRANSITION_TO_MC or
+                    self.extended_state.vtol_state ==
+                    ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
                 xy_radius = self.fw_rad
                 z_radius = self.fw_alt_rad
 
             if self.is_at_position(lat, lon, alt, xy_radius, z_radius):
-                rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" %
+                reached = True
-                    (index, count, self.last_pos_d, self.last_alt_d))
+                rospy.loginfo(
+                    "position reached | index: {0}, count: {1}, pos_d: {2}, alt_d: {3}".
+                    format(index, count, self.last_pos_d, self.last_alt_d))
                 break
 
-            count = count + 1
             self.rate.sleep()
 
         vtol_state_string = "VTOL undefined"
@@ -219,88 +244,79 @@ class MavrosMissionTest(unittest.TestCase):
             vtol_state_string = "VTOL MC"
         if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW):
             vtol_state_string = "VTOL FW"
-        if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC):
+        if (self.extended_state.vtol_state ==
+                ExtendedState.VTOL_STATE_TRANSITION_TO_MC):
             vtol_state_string = "VTOL FW->MC"
-        if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
+        if (self.extended_state.vtol_state ==
+                ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
             vtol_state_string = "VTOL MC->FW"
 
-        self.assertTrue(count < timeout, (("(%s) took too long to get to position " +
+        self.assertTrue(reached, (
-            "lat: %13.9f, lon: %13.9f, alt: %6.2f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f, VTOL state: %s") %
+            "({0}) took too long to get to position | lat: {1:13.9f}, lon: {2:13.9f}, alt: {3:6.2f}, xy off: {4}, z off: {5}, timeout: {6}, index: {7}, pos_d: {8}, alt_d: {9}, VTOL state: {10}".
-            (self.mission_name, lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d, vtol_state_string)))
+            format(self.mission_name, lat, lon, alt, xy_radius, z_radius,
-
+                   timeout, index, self.last_pos_d, self.last_alt_d,
-    def run_mission(self):
+                   vtol_state_string)))
-	# Hack to wait until vehicle is ready
-	# TODO better integration with pre-flight status reporting
-	time.sleep(5)
-	"""switch mode: auto and arm"""
-        self._srv_cmd_long(False, 176, False,
-                           # custom, auto, mission
-                           1, 4, 4, 0, 0, 0, 0)
-        # make sure the first command doesn't get lost
-        time.sleep(1)
-
-        self._srv_cmd_long(False, 400, False,
-                           # arm
-                           1, 0, 0, 0, 0, 0, 0)
 
     def wait_until_ready(self):
         """FIXME: hack to wait for simulation to be ready"""
-        while not self.has_global_pos:
+        rospy.loginfo("waiting for global position")
+        while not self.has_global_pos or math.isnan(
+                self.altitude.amsl) or math.isnan(self.altitude.relative):
             self.rate.sleep()
 
     def wait_on_landing(self, timeout, index):
         """Wait for landed state"""
-
+        rospy.loginfo("waiting for landing | timeout: {0}, index: {1}".format(
-        rospy.loginfo("waiting for landing " +
+            timeout, index))
-            "timeout: %d, index: %d" %
+        landed = False
-            (timeout, index))
+        for count in xrange(timeout):
-
-        count = 0
-        while count < timeout:
             if self.extended_state.landed_state == ExtendedState.LANDED_STATE_ON_GROUND:
+                rospy.loginfo(
+                    "landed | index: {0}, count: {1}".format(index, count))
+                landed = True
                 break
 
-            count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("(%s) landing not detected after landing WP " +
+        self.assertTrue(landed, (
-            "timeout: %d, index: %d") %
+            "({0}) landing not detected after landing WP | timeout: {1}, index: {2}".
-            (self.mission_name, timeout, index))
+            format(self.mission_name, timeout, index)))
 
     def wait_on_transition(self, transition, timeout, index):
         """Wait for VTOL transition"""
 
-        rospy.loginfo("waiting for VTOL transition " +
+        rospy.loginfo(
-            "transition: %d, timeout: %d, index: %d" %
+            "waiting for VTOL transition | transition: {0}, timeout: {1}, index: {2}".
-            (transition, timeout, index))
+            format(transition, timeout, index))
-
+        transitioned = False
-        count = 0
+        for count in xrange(timeout):
-        while count < timeout:
             # transition to MC
             if (transition == ExtendedState.VTOL_STATE_MC and
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC):
+                    self.extended_state.vtol_state ==
+                    ExtendedState.VTOL_STATE_MC):
+                rospy.loginfo("transitioned | index: {0}, count: {1}".format(
+                    index, count))
+                transitioned = True
                 break
 
             # transition to FW
             if (transition == ExtendedState.VTOL_STATE_FW and
-                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW):
+                    self.extended_state.vtol_state ==
+                    ExtendedState.VTOL_STATE_FW):
+                rospy.loginfo("transitioned | index: {0}, count: {1}".format(
+                    index, count))
+                transitioned = True
                 break
 
-            count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("(%s) transition not detected " +
+        self.assertTrue(transitioned, (
-            "timeout: %d, index: %d") %
+            "({0}) transition not detected | timeout: {1}, index: {2}".format(
-            (self.mission_name, timeout, index))
+                self.mission_name, timeout, index)))
-
-    def send_heartbeat(self, event=None):
-        # mav type gcs
-        mavmsg = mavutil.mavlink.MAVLink_heartbeat_message(6, 0, 0, 0, 0, 0)
-        # XXX: hack: using header object to set mav properties
-        mavmsg.pack(mavutil.mavlink.MAVLink_header(0, 0, 0, 2, 1))
-        rosmsg = mavlink.convert_to_rosmsg(mavmsg)
-        self.pub_mavlink.publish(rosmsg)
 
+    #
+    # Test method
+    #
     def test_mission(self):
         """Test mission"""
 
@@ -309,11 +325,11 @@ class MavrosMissionTest(unittest.TestCase):
             return
 
         self.mission_name = sys.argv[1]
-        mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
+        mission_file = os.path.dirname(
+            os.path.realpath(__file__)) + "/" + sys.argv[1]
 
-        rospy.loginfo("reading mission %s", mission_file)
+        rospy.loginfo("reading mission {0}".format(mission_file))
         wps = []
-
         with open(mission_file, 'r') as f:
             mission_ext = os.path.splitext(mission_file)[1]
             if mission_ext == '.mission':
@@ -330,35 +346,52 @@ class MavrosMissionTest(unittest.TestCase):
             else:
                 raise IOError('unknown mission file extension', mission_ext)
 
-        rospy.loginfo("wait until ready")
         self.wait_until_ready()
 
         rospy.loginfo("send mission")
-        res = self._srv_wp_push(wps)
+        result = False
-        rospy.loginfo(res)
+        try:
-        self.assertTrue(res.success, "(%s) mission could not be transfered" % self.mission_name)
+            res = self.wp_push_srv(start_index=0, waypoints=wps)
+            result = res.success
+        except rospy.ServiceException as e:
+            rospy.logerr(e)
+        self.assertTrue(
+            result,
+            "({0}) mission could not be transfered".format(self.mission_name))
+
+        rospy.loginfo("set mission mode and arm")
+        while self.state.mode != "AUTO.MISSION" or not self.state.armed:
+            if self.state.mode != "AUTO.MISSION":
+                try:
+                    res = self.set_mode_srv(0, 'AUTO.MISSION')
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+            if not self.state.armed:
+                try:
+                    self.set_arming_srv(True)
+                except rospy.ServiceException as e:
+                    rospy.logerr(e)
+            rospy.sleep(2)
 
         rospy.loginfo("run mission")
-        self.run_mission()
+        for index, waypoint in enumerate(wps):
-
-        index = 0
-        for waypoint in wps:
             # only check position for waypoints where this makes sense
             if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL:
                 alt = waypoint.z_alt
                 if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
-                    alt += self.home_alt
+                    alt += self.altitude.amsl - self.altitude.relative
 
-                self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, index)
+                self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600,
+                                    index)
 
             # check if VTOL transition happens if applicable
             if waypoint.command == 84 or waypoint.command == 85 or waypoint.command == 3000:
                 transition = waypoint.param1
 
-                if waypoint.command == 84: # VTOL takeoff implies transition to FW
+                if waypoint.command == 84:  # VTOL takeoff implies transition to FW
                     transition = ExtendedState.VTOL_STATE_FW
 
-                if waypoint.command == 85: # VTOL takeoff implies transition to MC
+                if waypoint.command == 85:  # VTOL takeoff implies transition to MC
                     transition = ExtendedState.VTOL_STATE_MC
 
                 self.wait_on_transition(transition, 600, index)
@@ -367,25 +400,32 @@ class MavrosMissionTest(unittest.TestCase):
             if waypoint.command == 85 or waypoint.command == 21:
                 self.wait_on_landing(600, index)
 
-            index += 1
+        if self.state.armed:
+            try:
+                self.set_arming_srv(False)
+            except rospy.ServiceException as e:
+                rospy.logerr(e)
 
         rospy.loginfo("mission done, calculating performance metrics")
         last_log = get_last_log()
-        rospy.loginfo("log file %s", last_log)
+        rospy.loginfo("log file {0}".format(last_log))
         data = px4tools.ulog.read_ulog(last_log).concat(dt=0.1)
         data = px4tools.ulog.compute_data(data)
         res = px4tools.estimator_analysis(data, False)
 
         # enforce performance
-        self.assertTrue(abs(res['roll_error_mean'])  < 5.0, str(res))
+        self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res))
         self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res))
         self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res))
         self.assertTrue(res['roll_error_std'] < 5.0, str(res))
         self.assertTrue(res['pitch_error_std'] < 5.0, str(res))
         self.assertTrue(res['yaw_error_std'] < 5.0, str(res))
 
+
 if __name__ == '__main__':
     import rostest
+    rospy.init_node('test_node', anonymous=True)
+
     name = "mavros_mission_test"
     if len(sys.argv) > 1:
         name += "-%s" % sys.argv[1]

launch/mavros_posix_tests_iris.launch

@@ -15,9 +15,9 @@
     </include>
 
     <group ns="$(arg ns)">
-        <test test-name="box" pkg="px4" type="mission_test.py" time-limit="120.0" args="multirotor_box.mission" />
+        <test test-name="box" pkg="px4" type="mission_test.py" time-limit="120.0" args="multirotor_box.mission"/>
-        <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" />
+        <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0"/>
-        <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
+        <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" time-limit="120.0"/>
     </group>
 </launch>