diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index 28e0f3b8d2..aa5bd9ae3f 100755
--- a/Tools/sitl_run.sh
+++ b/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
diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py
index 7329c1efcb..ec2b1bc5ae 100755
--- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py
+++ b/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")
diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
index 58e7ae2eda..8d6e9cd8bc 100755
--- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py
+++ b/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")
diff --git a/integrationtests/demo_tests/mavros_tests_posix.launch b/integrationtests/demo_tests/mavros_tests_posix.launch
new file mode 100644
index 0000000000..1f89bbfb03
--- /dev/null
+++ b/integrationtests/demo_tests/mavros_tests_posix.launch
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch
index f452f09453..7997a69d8a 100644
--- a/launch/mavros_sitl.launch
+++ b/launch/mavros_sitl.launch
@@ -3,12 +3,12 @@
-
-
-
-
-
+
+
+
+
+