first attempt to run mavros tests in new sitl environment

Tools/sitl_run.sh

@@ -33,7 +33,7 @@ then
 	model="iris"
 fi
 
-if [ "$#" != 5 ]
+if [ "$#" -lt 5 ]
 then
 	echo usage: sitl_run.sh rc_script debugger program model build_path
 	echo ""
@@ -52,6 +52,7 @@ fi
 
 set -e
 
+cd $build_path/..
 cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
 cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
 

integrationtests/demo_tests/mavros_offboard_attctl_test.py

@@ -41,15 +41,12 @@ import unittest
 import rospy
 import rosbag
 
-from px4.msg import vehicle_control_mode
-from px4.msg import vehicle_local_position
-from px4.msg import vehicle_attitude_setpoint
-from px4.msg import vehicle_attitude
 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 px4_test_helper import PX4TestHelper
+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
@@ -62,20 +59,21 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
     def setUp(self):
         rospy.init_node('test_node', anonymous=True)
         rospy.wait_for_service('mavros/cmd/arming', 30)
-        self.helper = PX4TestHelper("mavros_offboard_attctl_test")
-        self.helper.setUp()
+        #self.helper = PX4TestHelper("mavros_offboard_attctl_test")
+        #self.helper.setUp()
 
-        rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
-        rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback)
+        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.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_pos = False
-        self.control_mode = vehicle_control_mode()
         self.local_position = PoseStamped()
 
     def tearDown(self):
-        self.helper.tearDown()
+        #self.helper.tearDown()
+        pass
 
     #
     # General callback functions used in tests
@@ -84,9 +82,6 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
         self.has_pos = True
         self.local_position = data
 
-    def vehicle_control_mode_callback(self, data):
-        self.control_mode = data
-
     #
     # Test offboard position control
     #
@@ -101,6 +96,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
 
         throttle = Float64()
         throttle.data = 0.6
+        armed = False
 
         # does it cross expected boundaries in X seconds?
         count = 0
@@ -110,20 +106,23 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
             att.header.stamp = rospy.Time.now()
 
             self.pub_att.publish(att)
-            self.helper.bag_write('mavros/setpoint_attitude/attitude', 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.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle)
             self.rate.sleep()
 
+            # arm and switch to offboard
+            if not armed:
+                self._srv_cmd_long(False, 176, False,
+                                   128 | 1, 6, 0, 0, 0, 0, 0)
+                armed = True
+
             if (self.local_position.pose.position.x > 5
                     and self.local_position.pose.position.z > 5
                     and self.local_position.pose.position.y < -5):
                 break
             count = count + 1
 
-        self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set")
-        self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set")
-        self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
         self.assertTrue(count < timeout, "took too long to cross boundaries")
 
 

integrationtests/demo_tests/mavros_offboard_posctl_test.py

@@ -45,13 +45,10 @@ import rosbag
 from numpy import linalg
 import numpy as np
 
-from px4.msg import vehicle_control_mode
-from px4.msg import vehicle_local_position
-from px4.msg import vehicle_local_position_setpoint
 from std_msgs.msg import Header
 from geometry_msgs.msg import PoseStamped, Quaternion
 from tf.transformations import quaternion_from_euler
-from px4_test_helper import PX4TestHelper
+#from px4_test_helper import PX4TestHelper
 
 #
 # Tests flying a path in offboard control by sending position setpoints
@@ -64,19 +61,18 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
 
     def setUp(self):
         rospy.init_node('test_node', anonymous=True)
-        self.helper = PX4TestHelper("mavros_offboard_posctl_test")
-        self.helper.setUp()
+        #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
+        #self.helper.setUp()
 
-        rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
-        rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback)
+        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
         self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
         self.rate = rospy.Rate(10) # 10hz
         self.has_pos = False
         self.local_position = PoseStamped()
-        self.control_mode = vehicle_control_mode()
 
     def tearDown(self):
-        self.helper.tearDown()
+        #self.helper.tearDown()
+        pass
 
     #
     # General callback functions used in tests
@@ -85,9 +81,6 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
         self.has_pos = True
         self.local_position = data
 
-    def vehicle_control_mode_callback(self, data):
-        self.control_mode = data
-
 
     #
     # Helper methods
@@ -128,7 +121,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
             # update timestamp for each published SP
             pos.header.stamp = rospy.Time.now()
             self.pub_spt.publish(pos)
-            self.helper.bag_write('mavros/setpoint_position/local', pos)
+            #self.helper.bag_write('mavros/setpoint_position/local', pos)
 
             if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
                 break
@@ -160,9 +153,6 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
             count = count + 1
             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")
 
 

integrationtests/demo_tests/mavros_tests_posix.launch

@@ -0,0 +1,26 @@
+<launch>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="ns" default="iris"/>
+
+    <node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo iris $(find px4)/build_posix_sitl_default">
+        <env name="no_sim" value="1" />
+    </node>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="world_name" value="$(find px4)/Tools/sitl_gazebo/worlds/iris.world" />
+    </include>
+
+    <include file="$(find px4)/launch/mavros_sitl.launch">
+        <arg name="ns" value="$(arg ns)"/>
+        <arg name="fcu_url" value="udp://:14567@localhost:14557"/>
+        <arg name="tgt_component" value="1"/>
+    </include>
+
+    <group ns="$(arg ns)">
+        <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" />
+        <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
+    </group>
+</launch>

launch/mavros_sitl.launch

@@ -3,12 +3,12 @@
 <!-- example launch script for PX4 based FCU's -->
 
 <arg name="ns" default="/" />
-<group ns="$(arg ns)">
-	<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
-	<arg name="gcs_url" default="" />
-	<arg name="tgt_system" default="1" />
-	<arg name="tgt_component" default="50" />
+<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+<arg name="gcs_url" default="" />
+<arg name="tgt_system" default="1" />
+<arg name="tgt_component" default="50" />
 
+<group ns="$(arg ns)">
 	<include file="$(find mavros)/launch/node.launch">
 		<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
 		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />