diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch
index b5377659f9..34281b781a 100644
--- a/integrationtests/demo_tests/demo_tests.launch
+++ b/integrationtests/demo_tests/demo_tests.launch
@@ -1,8 +1,8 @@
-
-
+
+
-
+
@@ -12,7 +12,8 @@
+
-
+
diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py
index 4bf5085091..eb9144bbcb 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")
- 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
+
+ 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")
+ 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
similarity index 93%
rename from integrationtests/demo_tests/posctl_test.py
rename to integrationtests/demo_tests/offboard_posctl_test.py
index 1cbf09cf73..0d56082459 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 9d19fe5f9a..22bfb0c799 100644
--- a/launch/gazebo_ardrone_empty_world.launch
+++ b/launch/gazebo_ardrone_empty_world.launch
@@ -3,7 +3,7 @@
-
+