diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index 28e0f3b8d2a02bad51baf3dab9b5ead8d563bccc..aa5bd9ae3ff3d21012ecbf8bb25fb222d960ad0f 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 7329c1efcbee6151b123297957273d95d2a67403..ec2b1bc5ae51eae9b4bdbce4ce514b49524cf550 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 58e7ae2eda89ac021ce2d4ca1892e50be6e0c93f..8d6e9cd8bc18c83530ad9a578102adfb5f249155 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 0000000000000000000000000000000000000000..1f89bbfb039fa0146d2c1dcbefa5a2c2c81fc6cc
--- /dev/null
+++ b/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>
diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch
index f452f094531eab3fd99d02a2bf281e7455eebe0d..7997a69d8a9a66fce22d652fba33184b56503528 100644
--- a/launch/mavros_sitl.launch
+++ b/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" />