dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

first attempt to run mavros tests in new sitl environment

This commit is contained in:
Andreas Antener 2016-03-01 01:47:43 +01:00
parent f097e118df
commit cbc94fbe8c
5 changed files with 58 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/sitl_run.sh
View File

@ -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

37
integrationtests/demo_tests/mavros_offboard_attctl_test.py
View File

@ -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")

24
integrationtests/demo_tests/mavros_offboard_posctl_test.py
View File

@ -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")

26
integrationtests/demo_tests/mavros_tests_posix.launch Normal file
View File

@ -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>

10
launch/mavros_sitl.launch
View File

@ -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" />