mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-12 01:37:34 +08:00
reorganized posix sitl launch scripts, reorganized mavros python test scripts (integration tests) and updated them for posix sitl, removed old and not working integration tests
This commit is contained in:
Andreas Antener
@@ -0,0 +1,140 @@
|
||||
#!/usr/bin/env python
|
||||
#***************************************************************************
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
#***************************************************************************/
|
||||
|
||||
#
|
||||
# @author Andreas Antener <andreas@uaventure.com>
|
||||
#
|
||||
PKG = 'px4'
|
||||
|
||||
import unittest
|
||||
import rospy
|
||||
import rosbag
|
||||
|
||||
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 mavros_msgs.srv import CommandLong
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
#from px4_test_helper import PX4TestHelper
|
||||
|
||||
class MavrosOffboardAttctlTest(unittest.TestCase):
|
||||
"""
|
||||
Tests flying in offboard control by sending attitude and thrust setpoints
|
||||
via MAVROS.
|
||||
|
||||
For the test to be successful it needs to cross a certain boundary in time.
|
||||
"""
|
||||
|
||||
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()
|
||||
|
||||
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.local_position = PoseStamped()
|
||||
|
||||
def tearDown(self):
|
||||
#self.helper.tearDown()
|
||||
pass
|
||||
|
||||
#
|
||||
# General callback functions used in tests
|
||||
#
|
||||
def position_callback(self, data):
|
||||
self.local_position = data
|
||||
|
||||
def global_position_callback(self, data):
|
||||
self.has_global_pos = True
|
||||
|
||||
def test_attctl(self):
|
||||
"""Test offboard attitude control"""
|
||||
|
||||
# FIXME: hack to wait for simulation to be ready
|
||||
while not self.has_global_pos:
|
||||
self.rate.sleep()
|
||||
|
||||
# set some attitude and thrust
|
||||
att = PoseStamped()
|
||||
att.header = Header()
|
||||
att.header.frame_id = "base_footprint"
|
||||
att.header.stamp = rospy.Time.now()
|
||||
quaternion = quaternion_from_euler(0.25, 0.25, 0)
|
||||
att.pose.orientation = Quaternion(*quaternion)
|
||||
|
||||
throttle = Float64()
|
||||
throttle.data = 0.7
|
||||
armed = False
|
||||
|
||||
# does it cross expected boundaries in X seconds?
|
||||
count = 0
|
||||
timeout = 120
|
||||
while count < timeout:
|
||||
# update timestamp for each published SP
|
||||
att.header.stamp = rospy.Time.now()
|
||||
|
||||
self.pub_att.publish(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.rate.sleep()
|
||||
|
||||
# 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,
|
||||
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(count < timeout, "took too long to cross boundaries")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import rostest
|
||||
rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest)
|
||||
#unittest.main()
|
||||
@@ -0,0 +1,165 @@
|
||||
#!/usr/bin/env python
|
||||
#***************************************************************************
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
#***************************************************************************/
|
||||
|
||||
#
|
||||
# @author Andreas Antener <andreas@uaventure.com>
|
||||
#
|
||||
PKG = 'px4'
|
||||
|
||||
import unittest
|
||||
import rospy
|
||||
import math
|
||||
import rosbag
|
||||
|
||||
from numpy import linalg
|
||||
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 mavros_msgs.srv import CommandLong
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
#from px4_test_helper import PX4TestHelper
|
||||
|
||||
class MavrosOffboardPosctlTest(unittest.TestCase):
|
||||
"""
|
||||
Tests flying a path in offboard control by sending position setpoints
|
||||
via MAVROS.
|
||||
|
||||
For the test to be successful it needs to reach all setpoints in a certain time.
|
||||
FIXME: add flight path assertion (needs transformation from ROS frame to NED)
|
||||
"""
|
||||
|
||||
def setUp(self):
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
#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.local_position = PoseStamped()
|
||||
self.armed = False
|
||||
|
||||
def tearDown(self):
|
||||
#self.helper.tearDown()
|
||||
pass
|
||||
|
||||
#
|
||||
# General callback functions used in tests
|
||||
#
|
||||
def position_callback(self, data):
|
||||
self.local_position = data
|
||||
|
||||
def global_position_callback(self, data):
|
||||
self.has_global_pos = True
|
||||
|
||||
#
|
||||
# Helper methods
|
||||
#
|
||||
def is_at_position(self, x, y, z, offset):
|
||||
rospy.logdebug("current position %f, %f, %f" %
|
||||
(self.local_position.pose.position.x,
|
||||
self.local_position.pose.position.y,
|
||||
self.local_position.pose.position.z))
|
||||
|
||||
desired = np.array((x, y, z))
|
||||
pos = np.array((self.local_position.pose.position.x,
|
||||
self.local_position.pose.position.y,
|
||||
self.local_position.pose.position.z))
|
||||
return linalg.norm(desired - pos) < offset
|
||||
|
||||
def reach_position(self, x, y, z, timeout):
|
||||
# set a position setpoint
|
||||
pos = PoseStamped()
|
||||
pos.header = Header()
|
||||
pos.header.frame_id = "base_footprint"
|
||||
pos.pose.position.x = x
|
||||
pos.pose.position.y = y
|
||||
pos.pose.position.z = z
|
||||
|
||||
# For demo purposes we will lock yaw/heading to north.
|
||||
yaw_degrees = 0 # North
|
||||
yaw = math.radians(yaw_degrees)
|
||||
quaternion = quaternion_from_euler(0, 0, yaw)
|
||||
pos.pose.orientation = Quaternion(*quaternion)
|
||||
|
||||
# does it reach the position in X seconds?
|
||||
count = 0
|
||||
while count < timeout:
|
||||
# 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)
|
||||
|
||||
# FIXME: arm and switch to offboard
|
||||
# (need to wait the first few rounds until PX4 has the offboard stream)
|
||||
if not self.armed and count > 5:
|
||||
self._srv_cmd_long(False, 176, False,
|
||||
128 | 1, 6, 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
|
||||
count = count + 1
|
||||
self.rate.sleep()
|
||||
|
||||
self.assertTrue(count < timeout, "took too long to get to position")
|
||||
|
||||
def test_posctl(self):
|
||||
"""Test offboard position control"""
|
||||
|
||||
# FIXME: hack to wait for simulation to be 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))
|
||||
|
||||
for i in range(0, len(positions)):
|
||||
self.reach_position(positions[i][0], positions[i][1], positions[i][2], 180)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import rostest
|
||||
rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest)
|
||||
#unittest.main()
|
||||
@@ -0,0 +1 @@
|
||||
TODO: Adopt to new SITL
|
||||
@@ -0,0 +1,197 @@
|
||||
#!/usr/bin/env python
|
||||
#***************************************************************************
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
#***************************************************************************/
|
||||
|
||||
#
|
||||
# @author Andreas Antener <andreas@uaventure.com>
|
||||
#
|
||||
import rospy
|
||||
import threading
|
||||
|
||||
from px4.msg import vehicle_local_position
|
||||
from gazebo_msgs.srv import SpawnModel
|
||||
from gazebo_msgs.srv import SetModelState
|
||||
from gazebo_msgs.srv import DeleteModel
|
||||
from geometry_msgs.msg import Pose
|
||||
from geometry_msgs.msg import Twist
|
||||
|
||||
from numpy import linalg
|
||||
import numpy as np
|
||||
|
||||
#
|
||||
# Helper to test if vehicle stays on expected flight path.
|
||||
#
|
||||
class FlightPathAssertion(threading.Thread):
|
||||
|
||||
#
|
||||
# Arguments
|
||||
# - positions: tuple of tuples in the form (x, y, z, heading)
|
||||
#
|
||||
# TODO: yaw validation
|
||||
# TODO: fail main test thread
|
||||
#
|
||||
def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2):
|
||||
threading.Thread.__init__(self)
|
||||
rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback)
|
||||
self.spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
|
||||
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
|
||||
self.delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
|
||||
|
||||
self.positions = positions
|
||||
self.tunnel_radius = tunnelRadius
|
||||
self.yaw_offset = yaw_offset
|
||||
self.has_pos = False
|
||||
self.should_stop = False
|
||||
self.center = positions[0]
|
||||
self.end_of_segment = False
|
||||
self.failed = False
|
||||
self.local_position = vehicle_local_position
|
||||
|
||||
def position_callback(self, data):
|
||||
self.has_pos = True
|
||||
self.local_position = data
|
||||
|
||||
def spawn_indicator(self):
|
||||
self.delete_model("indicator")
|
||||
xml = (
|
||||
"<?xml version='1.0'?>" +
|
||||
"<sdf version='1.4'>" +
|
||||
"<model name='indicator'>" +
|
||||
"<static>true</static>" +
|
||||
"<link name='link'>" +
|
||||
"<visual name='visual'>" +
|
||||
"<transparency>0.7</transparency>" +
|
||||
"<geometry>" +
|
||||
"<sphere>" +
|
||||
"<radius>%f</radius>" +
|
||||
"</sphere>" +
|
||||
"</geometry>" +
|
||||
"<material>" +
|
||||
"<ambient>1 0 0 0.5</ambient>" +
|
||||
"<diffuse>1 0 0 0.5</diffuse>" +
|
||||
"</material>" +
|
||||
"</visual>" +
|
||||
"</link>" +
|
||||
"</model>" +
|
||||
"</sdf>") % self.tunnel_radius
|
||||
|
||||
self.spawn_model("indicator", xml, "", Pose(), "")
|
||||
|
||||
def position_indicator(self):
|
||||
state = SetModelState()
|
||||
state.model_name = "indicator"
|
||||
pose = Pose()
|
||||
pose.position.x = self.center[0]
|
||||
pose.position.y = (-1) * self.center[1]
|
||||
pose.position.z = (-1) * self.center[2]
|
||||
state.pose = pose
|
||||
state.twist = Twist()
|
||||
state.reference_frame = ""
|
||||
self.set_model_state(state)
|
||||
|
||||
def distance_to_line(self, a, b, pos):
|
||||
v = b - a
|
||||
w = pos - a
|
||||
|
||||
c1 = np.dot(w, v)
|
||||
if c1 <= 0: # before a
|
||||
self.center = a
|
||||
return linalg.norm(pos - a)
|
||||
|
||||
c2 = np.dot(v, v)
|
||||
if c2 <= c1: # after b
|
||||
self.center = b
|
||||
self.end_of_segment = True
|
||||
return linalg.norm(pos - b)
|
||||
|
||||
x = c1 / c2
|
||||
l = a + x * v
|
||||
self.center = l
|
||||
return linalg.norm(pos - l)
|
||||
|
||||
def stop(self):
|
||||
self.should_stop = True
|
||||
|
||||
def run(self):
|
||||
rate = rospy.Rate(10) # 10hz
|
||||
self.spawn_indicator()
|
||||
|
||||
current = 0
|
||||
count = 0
|
||||
while not self.should_stop:
|
||||
if self.has_pos:
|
||||
# calculate distance to line segment between first two points
|
||||
# if distances > tunnel_radius
|
||||
# exit with error
|
||||
# advance current pos if not on the line anymore or distance to next point < tunnel_radius
|
||||
# exit if current pos is now the last position
|
||||
|
||||
self.position_indicator()
|
||||
|
||||
pos = np.array((self.local_position.x,
|
||||
self.local_position.y,
|
||||
self.local_position.z))
|
||||
a_pos = np.array((self.positions[current][0],
|
||||
self.positions[current][1],
|
||||
self.positions[current][2]))
|
||||
b_pos = np.array((self.positions[current + 1][0],
|
||||
self.positions[current + 1][1],
|
||||
self.positions[current + 1][2]))
|
||||
|
||||
dist = self.distance_to_line(a_pos, b_pos, pos)
|
||||
b_dist = linalg.norm(pos - b_pos)
|
||||
|
||||
rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, b_dist))
|
||||
|
||||
if dist > self.tunnel_radius:
|
||||
msg = "left tunnel at position (%f, %f, %f)" % (self.local_position.x, self.local_position.y, self.local_position.z)
|
||||
rospy.logerr(msg)
|
||||
self.failed = True
|
||||
break
|
||||
|
||||
if self.end_of_segment or b_dist < self.tunnel_radius:
|
||||
rospy.loginfo("next segment")
|
||||
self.end_of_segment = False
|
||||
current = current + 1
|
||||
|
||||
if current == len(self.positions) - 1:
|
||||
rospy.loginfo("no more positions")
|
||||
break
|
||||
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
if count > 10 and not self.has_pos: # no position after 1 sec
|
||||
rospy.logerr("no position")
|
||||
self.failed = True
|
||||
break
|
||||
+167
@@ -0,0 +1,167 @@
|
||||
#!/usr/bin/env python
|
||||
#***************************************************************************
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
#***************************************************************************/
|
||||
|
||||
#
|
||||
# @author Andreas Antener <andreas@uaventure.com>
|
||||
#
|
||||
import rospy
|
||||
|
||||
from px4.msg import manual_control_setpoint
|
||||
from px4.msg import offboard_control_mode
|
||||
from mav_msgs.msg import CommandAttitudeThrust
|
||||
from std_msgs.msg import Header
|
||||
|
||||
#
|
||||
# Manual input control helper
|
||||
#
|
||||
class ManualInput(object):
|
||||
|
||||
def __init__(self):
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
self.pub_mcsp = rospy.Publisher('manual_control_setpoint', manual_control_setpoint, queue_size=10)
|
||||
self.pub_off = rospy.Publisher('offboard_control_mode', offboard_control_mode, queue_size=10)
|
||||
|
||||
def arm(self):
|
||||
rate = rospy.Rate(10) # 10hz
|
||||
|
||||
att = CommandAttitudeThrust()
|
||||
att.header = Header()
|
||||
|
||||
pos = manual_control_setpoint()
|
||||
pos.x = 0
|
||||
pos.z = 0
|
||||
pos.y = 0
|
||||
pos.r = 0
|
||||
pos.mode_switch = 3
|
||||
pos.return_switch = 3
|
||||
pos.posctl_switch = 3
|
||||
pos.loiter_switch = 3
|
||||
pos.acro_switch = 0
|
||||
pos.offboard_switch = 3
|
||||
|
||||
count = 0
|
||||
while not rospy.is_shutdown() and count < 5:
|
||||
rospy.loginfo("zeroing")
|
||||
time = rospy.get_rostime().now()
|
||||
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
|
||||
self.pub_mcsp.publish(pos)
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
pos.r = 1
|
||||
count = 0
|
||||
while not rospy.is_shutdown() and count < 5:
|
||||
rospy.loginfo("arming")
|
||||
time = rospy.get_rostime().now()
|
||||
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
|
||||
self.pub_mcsp.publish(pos)
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
def posctl(self):
|
||||
rate = rospy.Rate(10) # 10hz
|
||||
|
||||
# triggers posctl
|
||||
pos = manual_control_setpoint()
|
||||
pos.x = 0
|
||||
pos.z = 0
|
||||
pos.y = 0
|
||||
pos.r = 0
|
||||
pos.mode_switch = 2
|
||||
pos.return_switch = 3
|
||||
pos.posctl_switch = 1
|
||||
pos.loiter_switch = 3
|
||||
pos.acro_switch = 0
|
||||
pos.offboard_switch = 3
|
||||
|
||||
count = 0
|
||||
while not rospy.is_shutdown() and count < 5:
|
||||
rospy.loginfo("triggering posctl")
|
||||
time = rospy.get_rostime().now()
|
||||
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
|
||||
self.pub_mcsp.publish(pos)
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
|
||||
def offboard_attctl(self):
|
||||
self.offboard(False, False, True, True, True, True)
|
||||
|
||||
def offboard_posctl(self):
|
||||
self.offboard(False, False, True, False, True, True)
|
||||
|
||||
# Trigger offboard and set offboard control mode before
|
||||
def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True,
|
||||
ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True):
|
||||
rate = rospy.Rate(10) # 10hz
|
||||
|
||||
mode = offboard_control_mode()
|
||||
mode.ignore_thrust = ignore_thrust
|
||||
mode.ignore_attitude = ignore_attitude
|
||||
mode.ignore_bodyrate = ignore_bodyrate
|
||||
mode.ignore_position = ignore_position
|
||||
mode.ignore_velocity = ignore_velocity
|
||||
mode.ignore_acceleration_force = ignore_acceleration_force
|
||||
|
||||
count = 0
|
||||
while not rospy.is_shutdown() and count < 5:
|
||||
rospy.loginfo("setting offboard mode")
|
||||
time = rospy.get_rostime().now()
|
||||
mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
|
||||
self.pub_off.publish(mode)
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
# triggers offboard
|
||||
pos = manual_control_setpoint()
|
||||
pos.x = 0
|
||||
pos.z = 0
|
||||
pos.y = 0
|
||||
pos.r = 0
|
||||
pos.mode_switch = 3
|
||||
pos.return_switch = 3
|
||||
pos.posctl_switch = 3
|
||||
pos.loiter_switch = 3
|
||||
pos.acro_switch = 0
|
||||
pos.offboard_switch = 1
|
||||
|
||||
count = 0
|
||||
while not rospy.is_shutdown() and count < 5:
|
||||
rospy.loginfo("triggering offboard")
|
||||
time = rospy.get_rostime().now()
|
||||
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
|
||||
self.pub_mcsp.publish(pos)
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
@@ -0,0 +1,111 @@
|
||||
#!/usr/bin/env python
|
||||
#***************************************************************************
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
#***************************************************************************/
|
||||
|
||||
#
|
||||
# @author Andreas Antener <andreas@uaventure.com>
|
||||
#
|
||||
PKG = 'px4'
|
||||
|
||||
import unittest
|
||||
import rospy
|
||||
import rosbag
|
||||
|
||||
from px4.msg import vehicle_local_position
|
||||
from px4.msg import vehicle_attitude_setpoint
|
||||
from px4.msg import vehicle_attitude
|
||||
from px4.msg import vehicle_local_position_setpoint
|
||||
|
||||
from threading import Condition
|
||||
|
||||
#
|
||||
# Test helper
|
||||
#
|
||||
class PX4TestHelper(object):
|
||||
|
||||
def __init__(self, test_name):
|
||||
self.test_name = test_name
|
||||
|
||||
def setUp(self):
|
||||
self.condition = Condition()
|
||||
self.closed = False
|
||||
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4")
|
||||
|
||||
self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position",
|
||||
vehicle_local_position, self.vehicle_position_callback)
|
||||
self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint",
|
||||
vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback)
|
||||
self.sub_va = rospy.Subscriber("iris/vehicle_attitude",
|
||||
vehicle_attitude, self.vehicle_attitude_callback)
|
||||
self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint",
|
||||
vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback)
|
||||
|
||||
|
||||
def tearDown(self):
|
||||
try:
|
||||
self.condition.acquire()
|
||||
self.closed = True
|
||||
|
||||
self.sub_vlp.unregister()
|
||||
self.sub_vasp.unregister()
|
||||
self.sub_va.unregister()
|
||||
self.sub_vlps.unregister()
|
||||
self.bag.close()
|
||||
|
||||
finally:
|
||||
self.condition.release()
|
||||
|
||||
def vehicle_position_callback(self, data):
|
||||
self.bag_write('px4/vehicle_local_position', data)
|
||||
|
||||
def vehicle_attitude_setpoint_callback(self, data):
|
||||
self.bag_write('px4/vehicle_attitude_setpoint', data)
|
||||
|
||||
def vehicle_attitude_callback(self, data):
|
||||
self.bag_write('px4/vehicle_attitude', data)
|
||||
|
||||
def vehicle_local_position_setpoint_callback(self, data):
|
||||
self.bag_write('px4/vehicle_local_position_setpoint', data)
|
||||
|
||||
def bag_write(self, topic, data):
|
||||
try:
|
||||
self.condition.acquire()
|
||||
if not self.closed:
|
||||
self.bag.write(topic, data)
|
||||
else:
|
||||
rospy.logwarn("Trying to write to bag but it's already closed")
|
||||
finally:
|
||||
self.condition.release()
|
||||
|
||||
Reference in New Issue
Block a user