Skip to content
Snippets Groups Projects
Commit 5ce381df authored by Anthony Lamping's avatar Anthony Lamping Committed by Lorenz Meier
Browse files

update sitl tests

parent fefed35d
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py
+ 86
− 58
View file @ 5ce381df
...@@ -42,16 +42,13 @@ PKG = 'px4' ...@@ -42,16 +42,13 @@ PKG = 'px4'
import unittest import unittest
import rospy import rospy
import rosbag
import time
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 tf.transformations import quaternion_from_euler
from mavros_msgs.srv import CommandLong from geometry_msgs.msg import PoseStamped, Quaternion, Vector3
from mavros_msgs.msg import AttitudeTarget, State
from mavros_msgs.srv import CommandBool, SetMode
from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import NavSatFix
#from px4_test_helper import PX4TestHelper from std_msgs.msg import Header
class MavrosOffboardAttctlTest(unittest.TestCase): class MavrosOffboardAttctlTest(unittest.TestCase):
""" """
...@@ -62,23 +59,26 @@ class MavrosOffboardAttctlTest(unittest.TestCase): ...@@ -62,23 +59,26 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
""" """
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) self.rate = rospy.Rate(10) # 10hz
rospy.wait_for_service('mavros/cmd/arming', 30)
#self.helper = PX4TestHelper("mavros_offboard_attctl_test")
#self.helper.setUp()
rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_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_global_pos = False self.has_global_pos = False
self.local_position = PoseStamped() self.local_position = PoseStamped()
self.state = State()
# setup ROS topics and services
rospy.wait_for_service('mavros/cmd/arming', 30)
rospy.wait_for_service('mavros/set_mode', 30)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.Subscriber('mavros/local_position/pose', PoseStamped,
self.position_callback)
rospy.Subscriber('mavros/global_position/global', NavSatFix,
self.global_position_callback)
rospy.Subscriber('mavros/state', State, self.state_callback)
self.att_setpoint_pub = rospy.Publisher(
'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)
def tearDown(self): def tearDown(self):
#self.helper.tearDown()
pass pass
# #
...@@ -90,62 +90,90 @@ class MavrosOffboardAttctlTest(unittest.TestCase): ...@@ -90,62 +90,90 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
def global_position_callback(self, data): def global_position_callback(self, data):
self.has_global_pos = True self.has_global_pos = True
def test_attctl(self): def state_callback(self, data):
"""Test offboard attitude control""" self.state = data
# FIXME: hack to wait for simulation to be ready #
# Helper methods
#
def wait_until_ready(self):
"""FIXME: hack to wait for simulation to be ready"""
rospy.loginfo("waiting for global position")
while not self.has_global_pos: while not self.has_global_pos:
self.rate.sleep() self.rate.sleep()
#
# Test method
#
def test_attctl(self):
"""Test offboard attitude control"""
self.wait_until_ready()
# set some attitude and thrust # set some attitude and thrust
att = PoseStamped() att = AttitudeTarget()
att.header = Header() att.header = Header()
att.header.frame_id = "base_footprint" att.header.frame_id = "base_footprint"
att.header.stamp = rospy.Time.now() att.header.stamp = rospy.Time.now()
quaternion = quaternion_from_euler(0.25, 0.25, 0) att.orientation = Quaternion(*quaternion_from_euler(0.25, 0.25, 0))
att.pose.orientation = Quaternion(*quaternion) att.body_rate = Vector3()
att.thrust = 0.7
att.type_mask = 7
throttle = Float64() # send some setpoints before starting
throttle.data = 0.7 for i in xrange(20):
armed = False att.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(att)
self.rate.sleep()
rospy.loginfo("set mission mode and arm")
while self.state.mode != "OFFBOARD" or not self.state.armed:
if self.state.mode != "OFFBOARD":
try:
self.set_mode_srv(0, 'OFFBOARD')
except rospy.ServiceException as e:
rospy.logerr(e)
if not self.state.armed:
try:
self.set_arming_srv(True)
except rospy.ServiceException as e:
rospy.logerr(e)
rospy.sleep(2)
rospy.loginfo("run mission")
# does it cross expected boundaries in X seconds? # does it cross expected boundaries in X seconds?
count = 0
timeout = 120 timeout = 120
while count < timeout: crossed = False
for count in xrange(timeout):
# update timestamp for each published SP # update timestamp for each published SP
att.header.stamp = rospy.Time.now() att.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(att)
self.pub_att.publish(att) if (self.local_position.pose.position.x > 5 and
#self.helper.bag_write('mavros/setpoint_attitude/attitude', att) self.local_position.pose.position.z > 5 and
self.pub_thr.publish(throttle) self.local_position.pose.position.y < -5):
#self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle) rospy.loginfo("boundary crossed | count {0}".format(count))
self.rate.sleep() crossed = True
break
# FIXME: arm and switch to offboard
# (need to wait the first few rounds until PX4 has the offboard stream)
if not armed and count > 5:
self._srv_cmd_long(False, 176, False,
1, 6, 0, 0, 0, 0, 0)
# make sure the first command doesn't get lost
time.sleep(1)
self._srv_cmd_long(False, 400, False,
# arm
1, 0, 0, 0, 0, 0, 0)
armed = True self.rate.sleep()
if (self.local_position.pose.position.x > 5 self.assertTrue(crossed, (
and self.local_position.pose.position.z > 5 "took too long to cross boundaries | x: {0}, y: {1}, z: {2}, timeout: {3}".
and self.local_position.pose.position.y < -5): format(self.local_position.pose.position.x,
break self.local_position.pose.position.y,
count = count + 1 self.local_position.pose.position.z, timeout)))
self.assertTrue(count < timeout, "took too long to cross boundaries") if self.state.armed:
try:
self.set_arming_srv(False)
except rospy.ServiceException as e:
rospy.logerr(e)
if __name__ == '__main__': if __name__ == '__main__':
import rostest import rostest
rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest) rospy.init_node('test_node', anonymous=True)
#unittest.main()
rostest.rosrun(PKG, 'mavros_offboard_attctl_test',
MavrosOffboardAttctlTest)
integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py
+ 90
− 61
View file @ 5ce381df
...@@ -43,18 +43,14 @@ PKG = 'px4' ...@@ -43,18 +43,14 @@ PKG = 'px4'
import unittest import unittest
import rospy import rospy
import math import math
import rosbag
import time
from numpy import linalg
import numpy as np import numpy as np
from std_msgs.msg import Header
from geometry_msgs.msg import PoseStamped, Quaternion
from tf.transformations import quaternion_from_euler from tf.transformations import quaternion_from_euler
from mavros_msgs.srv import CommandLong from geometry_msgs.msg import PoseStamped, Quaternion
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode
from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import NavSatFix
#from px4_test_helper import PX4TestHelper from std_msgs.msg import Header
class MavrosOffboardPosctlTest(unittest.TestCase): class MavrosOffboardPosctlTest(unittest.TestCase):
""" """
...@@ -66,22 +62,26 @@ class MavrosOffboardPosctlTest(unittest.TestCase): ...@@ -66,22 +62,26 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
""" """
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) self.rate = rospy.Rate(10) # 10hz
#self.helper = PX4TestHelper("mavros_offboard_posctl_test")
#self.helper.setUp()
rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, 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_global_pos = False self.has_global_pos = False
self.local_position = PoseStamped() self.local_position = PoseStamped()
self.armed = False self.state = State()
# setup ROS topics and services
rospy.wait_for_service('mavros/cmd/arming', 30)
rospy.wait_for_service('mavros/set_mode', 30)
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
rospy.Subscriber('mavros/local_position/pose', PoseStamped,
self.position_callback)
rospy.Subscriber('mavros/global_position/global', NavSatFix,
self.global_position_callback)
rospy.Subscriber('mavros/state', State, self.state_callback)
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=10)
def tearDown(self): def tearDown(self):
#self.helper.tearDown()
pass pass
# #
...@@ -93,20 +93,28 @@ class MavrosOffboardPosctlTest(unittest.TestCase): ...@@ -93,20 +93,28 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
def global_position_callback(self, data): def global_position_callback(self, data):
self.has_global_pos = True self.has_global_pos = True
def state_callback(self, data):
self.state = data
# #
# Helper methods # Helper methods
# #
def wait_until_ready(self):
"""FIXME: hack to wait for simulation to be ready"""
rospy.loginfo("waiting for global position")
while not self.has_global_pos:
self.rate.sleep()
def is_at_position(self, x, y, z, offset): def is_at_position(self, x, y, z, offset):
rospy.logdebug("current position %f, %f, %f" % rospy.logdebug("current position | x:{0}, y:{1}, z:{2}".format(
(self.local_position.pose.position.x, self.local_position.pose.position.x, self.local_position.pose.
self.local_position.pose.position.y, position.y, self.local_position.pose.position.z))
self.local_position.pose.position.z))
desired = np.array((x, y, z)) desired = np.array((x, y, z))
pos = np.array((self.local_position.pose.position.x, pos = np.array((self.local_position.pose.position.x,
self.local_position.pose.position.y, self.local_position.pose.position.y,
self.local_position.pose.position.z)) self.local_position.pose.position.z))
return linalg.norm(desired - pos) < offset return np.linalg.norm(desired - pos) < offset
def reach_position(self, x, y, z, timeout): def reach_position(self, x, y, z, timeout):
# set a position setpoint # set a position setpoint
...@@ -123,54 +131,75 @@ class MavrosOffboardPosctlTest(unittest.TestCase): ...@@ -123,54 +131,75 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
quaternion = quaternion_from_euler(0, 0, yaw) quaternion = quaternion_from_euler(0, 0, yaw)
pos.pose.orientation = Quaternion(*quaternion) pos.pose.orientation = Quaternion(*quaternion)
# send some setpoints before starting
for i in xrange(20):
pos.header.stamp = rospy.Time.now()
self.pos_setpoint_pub.publish(pos)
self.rate.sleep()
rospy.loginfo("set mission mode and arm")
while self.state.mode != "OFFBOARD" or not self.state.armed:
if self.state.mode != "OFFBOARD":
try:
self.set_mode_srv(0, 'OFFBOARD')
except rospy.ServiceException as e:
rospy.logerr(e)
if not self.state.armed:
try:
self.set_arming_srv(True)
except rospy.ServiceException as e:
rospy.logerr(e)
rospy.sleep(2)
rospy.loginfo("run mission")
# does it reach the position in X seconds? # does it reach the position in X seconds?
count = 0 reached = False
while count < timeout: for count in xrange(timeout):
# 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.pos_setpoint_pub.publish(pos)
#self.helper.bag_write('mavros/setpoint_position/local', pos)
if self.is_at_position(pos.pose.position.x, pos.pose.position.y,
# FIXME: arm and switch to offboard pos.pose.position.z, 1):
# (need to wait the first few rounds until PX4 has the offboard stream) rospy.loginfo(
if not self.armed and count > 5: "position reached | count: {0}, x: {0}, y: {1}, z: {2}".
self._srv_cmd_long(False, 176, False, format(count, pos.pose.position.x, pos.pose.position.y,
1, 6, 0, 0, 0, 0, 0) pos.pose.position.z))
# make sure the first command doesn't get lost reached = True
time.sleep(1)
self._srv_cmd_long(False, 400, False,
# arm
1, 0, 0, 0, 0, 0, 0)
self.armed = True
if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 1):
break break
count = count + 1
self.rate.sleep() self.rate.sleep()
self.assertTrue(count < timeout, "took too long to get to position") self.assertTrue(reached, (
"took too long to get to position | x: {0}, y: {1}, z: {2}, timeout: {3}".
format(self.local_position.pose.position.x,
self.local_position.pose.position.y,
self.local_position.pose.position.z, timeout)))
#
# Test method
#
def test_posctl(self): def test_posctl(self):
"""Test offboard position control""" """Test offboard position control"""
# FIXME: hack to wait for simulation to be ready self.wait_until_ready()
while not self.has_global_pos:
self.rate.sleep() positions = ((0, 0, 0), (2, 2, 2), (2, -2, 2), (-2, -2, 2), (2, 2, 2))
positions = ( for i in xrange(len(positions)):
(0, 0, 0), self.reach_position(positions[i][0], positions[i][1],
(2, 2, 2), positions[i][2], 180)
(2, -2, 2),
(-2, -2, 2),
(2, 2, 2))
for i in range(0, len(positions)): if self.state.armed:
self.reach_position(positions[i][0], positions[i][1], positions[i][2], 180) try:
self.set_arming_srv(False)
except rospy.ServiceException as e:
rospy.logerr(e)
if __name__ == '__main__': if __name__ == '__main__':
import rostest import rostest
rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest) rospy.init_node('test_node', anonymous=True)
#unittest.main()
rostest.rosrun(PKG, 'mavros_offboard_posctl_test',
MavrosOffboardPosctlTest)
integrationtests/python_src/px4_it/mavros/mission_test.py
+ 187
− 147
View file @ 5ce381df
This diff is collapsed.
launch/mavros_posix_tests_iris.launch
+ 3
− 3
View file @ 5ce381df
...@@ -15,9 +15,9 @@ ...@@ -15,9 +15,9 @@
</include> </include>
<group ns="$(arg ns)"> <group ns="$(arg ns)">
<test test-name="box" pkg="px4" type="mission_test.py" time-limit="120.0" args="multirotor_box.mission" /> <test test-name="box" pkg="px4" type="mission_test.py" time-limit="120.0" args="multirotor_box.mission"/>
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" /> <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0"/>
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" time-limit="120.0"/>
</group> </group>
</launch> </launch>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment