- updated manual input to publish directly to px4 and not over joystick topics

- updated tests to work with current setup

4 files changed, 84 insertions, 32 deletions

diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch
index b5377659f..34281b781 100644
--- a/integrationtests/demo_tests/demo_tests.launch
+++ b/integrationtests/demo_tests/demo_tests.launch
@@ -1,8 +1,8 @@
 <launch>
-	<arg name="headless" default="true"/>
-  	<arg name="gui" default="false"/>
+	<arg name="headless" default="false"/>
+  	<arg name="gui" default="true"/>
   	<arg name="enable_logging" default="false"/>
-    <arg name="enable_ground_truth" default="true"/>
+    <arg name="enable_ground_truth" default="false"/>
     <arg name="log_file" default="iris"/>
 
 	<include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
@@ -12,7 +12,8 @@
         <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
         <arg name="log_file" value="$(arg log_file)"/>
 	</include>
+	<include file="$(find px4)/launch/mavros_sitl.launch" />
 
 	<test test-name="arm" pkg="px4" type="arm_test.py" />
-	<test test-name="posctl" pkg="px4" type="posctl_test.py" />
+	<test test-name="offboard_posctl" pkg="px4" type="offboard_posctl_test.py" />
 </launch>
diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py
index 4bf508509..eb9144bbc 100755
--- a/integrationtests/demo_tests/manual_input.py
+++ b/integrationtests/demo_tests/manual_input.py
@@ -2,43 +2,59 @@
 import sys
 import rospy
 
-from sensor_msgs.msg import Joy
+from px4.msg import manual_control_setpoint
+from mav_msgs.msg import CommandAttitudeThrust
 from std_msgs.msg import Header
 
-
 #
-# Manual input control helper, fakes joystick input
-# > needs to correspond to default mapping in manual_input node
+# Manual input control helper
+#
+# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else
+# the simulator does not instantiate our controller.
 #
 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)
+		self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
+		self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, 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] 
+		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 < 10:
 			rospy.loginfo("zeroing")
-			self.joyPx4.publish(msg)
-			self.joyIris.publish(msg)
+			time = rospy.get_rostime().now()
+			pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+			self.pubMcsp.publish(pos)
+			# Fake input to iris commander
+			self.pubAtt.publish(att)
 			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] 
+		pos.r = 1
 		count = 0
 		while not rospy.is_shutdown() and count < 10:
 			rospy.loginfo("arming")
-			self.joyPx4.publish(msg)
-			self.joyIris.publish(msg)
+			time = rospy.get_rostime().now()
+			pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+			self.pubMcsp.publish(pos)
 			rate.sleep()
 			count = count + 1
 
@@ -46,14 +62,49 @@ class ManualInput:
 		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] 
+		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 < 10:
+			rospy.loginfo("triggering posctl")
+			time = rospy.get_rostime().now()
+			pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+			self.pubMcsp.publish(pos)
+			rate.sleep()
+			count = count + 1
+
+	def offboard(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 = 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 < 10:
 			rospy.loginfo("triggering posctl")
-			self.joyPx4.publish(msg)
-			self.joyIris.publish(msg)
+			time = rospy.get_rostime().now()
+			pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
+			self.pubMcsp.publish(pos)
 			rate.sleep()
 			count = count + 1
+
diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py
index 1cbf09cf7..0d5608245 100755
--- a/integrationtests/demo_tests/posctl_test.py
+++ b/integrationtests/demo_tests/offboard_posctl_test.py
@@ -16,7 +16,7 @@ from std_msgs.msg import Header
 from manual_input import ManualInput
 
 
-class PosctlTest(unittest.TestCase):
+class OffboardPosctlTest(unittest.TestCase):
 
 	#
 	# General callback functions used in tests
@@ -37,7 +37,7 @@ class PosctlTest(unittest.TestCase):
 		return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset)
 
 	#
-	# Test POSCTL
+	# Test offboard POSCTL
 	#
 	def test_posctl(self):
 		rospy.init_node('test_node', anonymous=True)
@@ -48,9 +48,9 @@ class PosctlTest(unittest.TestCase):
 
 		manIn = ManualInput()
 
-		# arm and go into POSCTL
+		# arm and go into offboard
 		manIn.arm()
-		manIn.posctl()
+		manIn.offboard()
 		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")
@@ -93,4 +93,4 @@ class PosctlTest(unittest.TestCase):
 
 if __name__ == '__main__':
 	import rostest
-	rostest.rosrun(PKG, 'posctl_test', PosctlTest)
+	rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest)
diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch
index 9d19fe5f9..22bfb0c79 100644
--- a/launch/gazebo_ardrone_empty_world.launch
+++ b/launch/gazebo_ardrone_empty_world.launch
@@ -3,7 +3,7 @@
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="enable_logging" default="false"/>
-    <arg name="enable_ground_truth" default="true"/>
+    <arg name="enable_ground_truth" default="false"/>
     <arg name="log_file" default="ardrone"/>
 
     <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch">