diff options
Diffstat (limited to 'integrationtests')
-rwxr-xr-x | integrationtests/demo_tests/direct_offboard_posctl_test.py | 35 | ||||
-rw-r--r-- | integrationtests/demo_tests/flight_path_assertion.py | 23 |
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): |