- publish offboard mode with manual input so the vehicle control status is correct

- trigger offboard in mavros attitude test

1 files changed, 31 insertions, 2 deletions

diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py
index cf139bb1d..9b2471a00 100755
--- a/integrationtests/demo_tests/manual_input.py
+++ b/integrationtests/demo_tests/manual_input.py
@@ -39,6 +39,7 @@ import sys
 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
 
@@ -46,13 +47,15 @@ from std_msgs.msg import Header
 # Manual input control helper
 #
 # FIXME: 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.
+# the simulator does not instantiate our controller. Probably this whole class will disappear once
+# arming works correctly.
 #
 class ManualInput:
 
     def __init__(self):
         rospy.init_node('test_node', anonymous=True)
         self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
+        self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10)
         self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
 
     def arm(self):
@@ -119,9 +122,35 @@ class ManualInput:
             rate.sleep()
             count = count + 1
 
-    def offboard(self):
+
+    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.pubOff.publish(mode)
+            rate.sleep()
+            count = count + 1
+
         # triggers offboard
         pos = manual_control_setpoint()
         pos.x = 0