Commit a54849ee authored by Andreas Antener's avatar Andreas Antener Committed by Thomas Gubler
Browse files

adding previous integration demo tests

parent cbbc660b
......@@ -134,6 +134,7 @@ include_directories(
src/
src/lib
${EIGEN_INCLUDE_DIRS}
integrationtests
)
## generate multiplatform wrapper headers
......@@ -320,3 +321,10 @@ install(TARGETS ${PROJECT_NAME}
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest(integrationtests/integrationtests.launch)
endif()
#!/usr/bin/env python
PKG = 'px4'
import sys
import unittest
import rospy
from px4.msg import actuator_armed
from manual_input import ManualInput
class ArmTest(unittest.TestCase):
#
# General callback functions used in tests
#
def actuator_armed_callback(self, data):
self.actuatorStatus = data
#
# Test arming
#
def test_arm(self):
rospy.init_node('test_node', anonymous=True)
sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback)
# method to test
arm = ManualInput()
arm.arm()
self.assertEquals(self.actuatorStatus.armed, True, "not armed")
if __name__ == '__main__':
import rostest
rostest.rosrun(PKG, 'arm_test', ArmTest)
<launch>
<arg name="headless" default="true"/>
<arg name="gui" default="false"/>
<arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="iris"/>
<include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
<arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
</include>
<test test-name="arm" pkg="px4" type="arm_test.py" />
<test test-name="posctl" pkg="px4" type="posctl_test.py" />
</launch>
#!/usr/bin/env python
import sys
import rospy
from sensor_msgs.msg import Joy
from std_msgs.msg import Header
#
# Manual input control helper, fakes joystick input
# > needs to correspond to default mapping in manual_input node
#
class ManualInput:
def __init__(self):
rospy.init_node('test_node', anonymous=True)
self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10)
self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10)
def arm(self):
rate = rospy.Rate(10) # 10hz
msg = Joy()
msg.header = Header()
msg.buttons = [0, 0, 0, 0, 0]
msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("zeroing")
self.joyPx4.publish(msg)
self.joyIris.publish(msg)
rate.sleep()
count = count + 1
msg.buttons = [0, 0, 0, 0, 0]
msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("arming")
self.joyPx4.publish(msg)
self.joyIris.publish(msg)
rate.sleep()
count = count + 1
def posctl(self):
rate = rospy.Rate(10) # 10hz
# triggers posctl
msg = Joy()
msg.header = Header()
msg.buttons = [0, 0, 1, 0, 0]
msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("triggering posctl")
self.joyPx4.publish(msg)
self.joyIris.publish(msg)
rate.sleep()
count = count + 1
#!/usr/bin/env python
PKG = 'px4'
import sys
import unittest
import rospy
from px4.msg import vehicle_local_position
from px4.msg import vehicle_control_mode
from px4.msg import actuator_armed
from px4.msg import position_setpoint_triplet
from px4.msg import position_setpoint
from sensor_msgs.msg import Joy
from std_msgs.msg import Header
from manual_input import ManualInput
class PosctlTest(unittest.TestCase):
#
# General callback functions used in tests
#
def position_callback(self, data):
self.hasPos = True
self.localPosition = data
def vehicle_control_mode_callback(self, data):
self.controlMode = data
#
# Helper methods
#
def is_at_position(self, x, y, z, offset):
rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z))
return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset)
#
# Test POSCTL
#
def test_posctl(self):
rospy.init_node('test_node', anonymous=True)
rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
rate = rospy.Rate(10) # 10hz
manIn = ManualInput()
# arm and go into POSCTL
manIn.arm()
manIn.posctl()
self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set")
self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
# set a position setpoint
pos = position_setpoint()
pos.valid = True
pos.x = 2
pos.z = -2
pos.y = 2
pos.position_valid = True
stp = position_setpoint_triplet()
stp.current = pos
pubSpt.publish(stp)
# does it reach the position in X seconds?
count = 0
timeout = 120
while(count < timeout):
if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)):
break
count = count + 1
rate.sleep()
self.assertTrue(count < timeout, "took too long to get to position")
# does it hold the position for Y seconds?
positionHeld = True
count = 0
timeout = 50
while(count < timeout):
if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)):
positionHeld = False
break
count = count + 1
rate.sleep()
self.assertTrue(count == timeout, "position could not be held")
if __name__ == '__main__':
import rostest
rostest.rosrun(PKG, 'posctl_test', PosctlTest)
<launch>
<arg name="headless" default="true"/>
<arg name="gui" default="false"/>
<arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="false"/>
<arg name="log_file" default="iris"/>
<include file="$(find px4)/integrationtests/demo_tests/demo_tests.launch">
<arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
</include>
</launch>
......@@ -46,6 +46,7 @@
<build_depend>eigen</build_depend>
<build_depend>libmavconn</build_depend>
<build_depend>tf</build_depend>
<build_depend>rostest</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
......
Supports Markdown
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