Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

first attempt to run mavros tests in new sitl environment

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
Tools/sitl_run.sh
+2 -1
View File
@@ -33,7 +33,7 @@ then
model="iris" model="iris"
fi fi
if [ "$#" != 5 ] if [ "$#" -lt 5 ]
then then
echo usage: sitl_run.sh rc_script debugger program model build_path echo usage: sitl_run.sh rc_script debugger program model build_path
echo "" echo ""
@@ -52,6 +52,7 @@ fi
set -e set -e
cd $build_path/..
cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
integrationtests/demo_tests/mavros_offboard_attctl_test.py
+18 -19
View File
@@ -41,15 +41,12 @@ import unittest
import rospy import rospy
import rosbag 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 Header
from std_msgs.msg import Float64 from std_msgs.msg import Float64
from geometry_msgs.msg import PoseStamped, Quaternion from geometry_msgs.msg import PoseStamped, Quaternion
from tf.transformations import quaternion_from_euler 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 # Tests flying a path in offboard control by sending attitude and thrust setpoints
@@ -62,20 +59,21 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
rospy.wait_for_service('mavros/cmd/arming', 30) rospy.wait_for_service('mavros/cmd/arming', 30)
self.helper = PX4TestHelper("mavros_offboard_attctl_test") #self.helper = PX4TestHelper("mavros_offboard_attctl_test")
self.helper.setUp() #self.helper.setUp()
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback)
self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) 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) 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.rate = rospy.Rate(10) # 10hz
self.has_pos = False self.has_pos = False
self.control_mode = vehicle_control_mode()
self.local_position = PoseStamped() self.local_position = PoseStamped()
def tearDown(self): def tearDown(self):
self.helper.tearDown() #self.helper.tearDown()
pass
# #
# General callback functions used in tests # General callback functions used in tests
@@ -84,9 +82,6 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
self.has_pos = True self.has_pos = True
self.local_position = data self.local_position = data
def vehicle_control_mode_callback(self, data):
self.control_mode = data
# #
# Test offboard position control # Test offboard position control
# #
@@ -101,6 +96,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
throttle = Float64() throttle = Float64()
throttle.data = 0.6 throttle.data = 0.6
armed = False
# does it cross expected boundaries in X seconds? # does it cross expected boundaries in X seconds?
count = 0 count = 0
@@ -110,20 +106,23 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
att.header.stamp = rospy.Time.now() att.header.stamp = rospy.Time.now()
self.pub_att.publish(att) 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.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() 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 if (self.local_position.pose.position.x > 5
and self.local_position.pose.position.z > 5 and self.local_position.pose.position.z > 5
and self.local_position.pose.position.y < -5): and self.local_position.pose.position.y < -5):
break break
count = count + 1 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") self.assertTrue(count < timeout, "took too long to cross boundaries")
integrationtests/demo_tests/mavros_offboard_posctl_test.py
+7 -17
View File
@@ -45,13 +45,10 @@ import rosbag
from numpy import linalg from numpy import linalg
import numpy as np 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 std_msgs.msg import Header
from geometry_msgs.msg import PoseStamped, Quaternion from geometry_msgs.msg import PoseStamped, Quaternion
from tf.transformations import quaternion_from_euler 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 # Tests flying a path in offboard control by sending position setpoints
@@ -64,19 +61,18 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
self.helper = PX4TestHelper("mavros_offboard_posctl_test") #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
self.helper.setUp() #self.helper.setUp()
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback)
self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
self.rate = rospy.Rate(10) # 10hz self.rate = rospy.Rate(10) # 10hz
self.has_pos = False self.has_pos = False
self.local_position = PoseStamped() self.local_position = PoseStamped()
self.control_mode = vehicle_control_mode()
def tearDown(self): def tearDown(self):
self.helper.tearDown() #self.helper.tearDown()
pass
# #
# General callback functions used in tests # General callback functions used in tests
@@ -85,9 +81,6 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
self.has_pos = True self.has_pos = True
self.local_position = data self.local_position = data
def vehicle_control_mode_callback(self, data):
self.control_mode = data
# #
# Helper methods # Helper methods
@@ -128,7 +121,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
# update timestamp for each published SP # update timestamp for each published SP
pos.header.stamp = rospy.Time.now() pos.header.stamp = rospy.Time.now()
self.pub_spt.publish(pos) 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): if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
break break
@@ -160,9 +153,6 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
count = count + 1 count = count + 1
self.rate.sleep() 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") self.assertTrue(count == timeout, "position could not be held")
integrationtests/demo_tests/mavros_tests_posix.launch
+26
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>
launch/mavros_sitl.launch
+5 -5
View File
@@ -3,12 +3,12 @@
<!-- example launch script for PX4 based FCU's --> <!-- example launch script for PX4 based FCU's -->
<arg name="ns" default="/" /> <arg name="ns" default="/" />
<group ns="$(arg ns)"> <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" /> <arg name="gcs_url" default="" />
<arg name="gcs_url" default="" /> <arg name="tgt_system" default="1" />
<arg name="tgt_system" default="1" /> <arg name="tgt_component" default="50" />
<arg name="tgt_component" default="50" />
<group ns="$(arg ns)">
<include file="$(find mavros)/launch/node.launch"> <include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" /> <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" /> <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />