update flight path assertion and error handling

2 files changed, 37 insertions, 21 deletions

diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py
index af8d4b821..6b28e0a18 100755
--- a/integrationtests/demo_tests/direct_offboard_posctl_test.py
+++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py
@@ -29,6 +29,10 @@ class OffboardPosctlTest(unittest.TestCase):
 		self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
 		self.rate = rospy.Rate(10) # 10hz
 
+	def tearDown(self):
+		if (self.fpa):
+			self.fpa.stop()
+
 	#
 	# General callback functions used in tests
 	#
@@ -83,22 +87,21 @@ class OffboardPosctlTest(unittest.TestCase):
 		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")
-	
+
 		# prepare flight path assertion
-		fpa = FlightPathAssertion(
-			(
-				(0,0,0),
-				(2,2,-2),
-				(2,-2,-2),
-				(-2,-2,-2),
-				(2,2,-2),
-			), 0.5, 0)
-		fpa.start()
-
-		self.reach_position(2, 2, -2, 120)
-		self.reach_position(2, -2, -2, 120)
-		self.reach_position(-2, -2, -2, 120)
-		self.reach_position(2, 2, -2, 120)
+		positions = (
+			(0,0,0),
+			(2,2,-2),
+			(2,-2,-2),
+			(-2,-2,-2),
+			(2,2,-2))
+
+		self.fpa = FlightPathAssertion(positions, 1, 0)
+		self.fpa.start()
+
+		for i in range(0, len(positions)):
+			self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+			self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i)
 		
 		# does it hold the position for Y seconds?
 		positionHeld = True
@@ -112,7 +115,7 @@ class OffboardPosctlTest(unittest.TestCase):
 			self.rate.sleep()
 
 		self.assertTrue(count == timeout, "position could not be held")
-		fpa.stop()
+		self.fpa.stop()
 	
 
 if __name__ == '__main__':
diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py
index fb78f8929..1d99b7e5a 100644
--- a/integrationtests/demo_tests/flight_path_assertion.py
+++ b/integrationtests/demo_tests/flight_path_assertion.py
@@ -6,6 +6,7 @@ import threading
 from px4.msg import vehicle_local_position
 from gazebo_msgs.srv import SpawnModel
 from gazebo_msgs.srv import SetModelState
+from gazebo_msgs.srv import DeleteModel
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Twist
 
@@ -18,11 +19,20 @@ import math
 #
 class FlightPathAssertion(threading.Thread):
 
+    #
+    # Arguments
+    # - positions: tuple of tuples in the form (x, y, z, heading)
+    #
+    # TODO: yaw validation
+    # TODO: fail main test thread
+    #
     def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2):
         threading.Thread.__init__(self)
         rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback)
-        self.spawn = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
+        self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
         self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
+        self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
+
         self.positions = positions
         self.tunnelRadius = tunnelRadius
         self.yawOffset = yawOffset
@@ -30,14 +40,16 @@ class FlightPathAssertion(threading.Thread):
         self.shouldStop = False
         self.center = positions[0]
         self.endOfSegment = False
+        self.failed = False
 
     def position_callback(self, data):
         self.hasPos = True
         self.localPosition = data
 
     def spawn_indicator(self):
+        self.deleteModel("indicator")
         xml = "<?xml version='1.0'?><sdf version='1.4'><model name='indicator'><static>true</static><link name='link'><visual name='visual'><transparency>0.7</transparency><geometry><sphere><radius>%f</radius></sphere></geometry><material><ambient>1 0 0 0.5</ambient><diffuse>1 0 0 0.5</diffuse></material></visual></link></model></sdf>" % self.tunnelRadius
-        self.spawn("indicator", xml, "", Pose(), "")
+        self.spawnModel("indicator", xml, "", Pose(), "")
 
     def position_indicator(self):
         state = SetModelState()
@@ -103,11 +115,12 @@ class FlightPathAssertion(threading.Thread):
                 dist = self.distance_to_line(aPos, bPos, pos)
                 bDist = linalg.norm(pos - bPos)
 
-                rospy.loginfo("distance to line: %f, distance to end: %f" % (dist, bDist))
+                rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist))
 
                 if (dist > self.tunnelRadius):
-                    rospy.logerr("left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z))
-                    # FIXME: assertion
+                    msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)
+                    rospy.logerr(msg)
+                    self.failed = True
                     break
 
                 if (self.endOfSegment or bDist < self.tunnelRadius):