Merge pull request #1863 from UAVenture/offboard_attitude_pub

Trigger correct offboard mode in mavros IT

3 files changed, 34 insertions, 4 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
diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py
index 27885635a..a52f7ffc1 100755
--- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py
+++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py
@@ -100,6 +100,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
         # FIXME: this must go ASAP when arming is implemented
         manIn = ManualInput()
         manIn.arm()
+        manIn.offboard_attctl()
 
         self.assertTrue(self.arm(), "Could not arm")
         self.rateSec.sleep()
@@ -111,7 +112,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
         att.header = Header() 
         att.header.frame_id = "base_footprint"
         att.header.stamp = rospy.Time.now()
-        quaternion = quaternion_from_euler(0.2, 0.2, 0)
+        quaternion = quaternion_from_euler(0.15, 0.15, 0)
         att.pose.orientation = Quaternion(*quaternion)
 
         throttle = Float64()
diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
index a1f1cf3c5..a3739ae5c 100755
--- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py
+++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
@@ -136,7 +136,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
         # FIXME: this must go ASAP when arming is implemented
         manIn = ManualInput()
         manIn.arm()
-        manIn.offboard()
+        manIn.offboard_posctl()
 
         self.assertTrue(self.arm(), "Could not arm")
         self.rateSec.sleep()