adding previous integration demo tests

CMakeLists.txt

@@ -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()
+
+

integrationtests/demo_tests/arm_test.py

@@ -0,0 +1,37 @@
+#!/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)

integrationtests/demo_tests/demo_tests.launch

@@ -0,0 +1,18 @@
+<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>

integrationtests/demo_tests/manual_input.py

@@ -0,0 +1,59 @@
+#!/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

integrationtests/demo_tests/posctl_test.py

@@ -0,0 +1,96 @@
+#!/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)

integrationtests/integrationtests.launch

@@ -0,0 +1,15 @@
+<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>

package.xml

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