diff options
author | Andreas Antener <antener_a@gmx.ch> | 2015-02-23 21:45:01 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 18:25:36 +0100 |
commit | 952f91738ea0704fe5dbbb1598a17ca53f3a70a0 (patch) | |
tree | 5009242d88def69dd08a6ea4260c43b357542902 /integrationtests/demo_tests/flight_path_assertion.py | |
parent | 85ac3e3515bc214a770074182617208b24ee0209 (diff) | |
download | px4-firmware-952f91738ea0704fe5dbbb1598a17ca53f3a70a0.tar.gz px4-firmware-952f91738ea0704fe5dbbb1598a17ca53f3a70a0.tar.bz2 px4-firmware-952f91738ea0704fe5dbbb1598a17ca53f3a70a0.zip |
update flight path assertion and error handling
Diffstat (limited to 'integrationtests/demo_tests/flight_path_assertion.py')
-rw-r--r-- | integrationtests/demo_tests/flight_path_assertion.py | 23 |
1 files changed, 18 insertions, 5 deletions
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): |