def test_posctl(self): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() manIn.offboard_posctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # prepare flight path positions = ( (0,0,0), (2,2,2), (2,-2,2), (-2,-2,2), (2,2,2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) # does it hold the position for Y seconds? positionHeld = True count = 0 timeout = 50 while(count < timeout): if(not self.is_at_position(2, 2, 2, 0.5)): positionHeld = False break count = count + 1 self.rate.sleep() self.assertTrue(count == timeout, "position could not be held")
def test_posctl(self): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() manIn.offboard_posctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # prepare flight path positions = ((0, 0, 0), (2, 2, 2), (2, -2, 2), (-2, -2, 2), (2, 2, 2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) # does it hold the position for Y seconds? positionHeld = True count = 0 timeout = 50 while (count < timeout): if (not self.is_at_position(2, 2, 2, 0.5)): positionHeld = False break count = count + 1 self.rate.sleep() self.assertTrue(count == timeout, "position could not be held")
def test_manual_input(self): rospy.init_node('test_node', anonymous=True) rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) man_in = ManualInput() # Test arming man_in.arm() self.assertEquals(self.actuator_status.armed, True, "did not arm") # Test posctl man_in.posctl() self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # Test offboard man_in.offboard() self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
def test_attctl(self): # 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() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # set some attitude and thrust att = PoseStamped() att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) att.pose.orientation = Quaternion(*quaternion) throttle = Float64() throttle.data = 0.6 # does it cross expected boundaries in X seconds? count = 0 timeout = 120 while (count < timeout): # update timestamp for each published SP att.header.stamp = rospy.Time.now() self.pubAtt.publish(att) self.pubThr.publish(throttle) if (self.localPosition.pose.position.x > 5 and self.localPosition.pose.position.z > 5 and self.localPosition.pose.position.y < -5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to cross boundaries")
def test_posctl(self): manIn = ManualInput() # arm and go into offboard manIn.arm() manIn.offboard() 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 positions = ( (0,0,0), (2,2,-2), (2,-2,-2), (-2,-2,-2), (2,2,-2)) # flight path assertion 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 count = 0 timeout = 50 while(count < timeout): if(not self.is_at_position(2, 2, -2, 0.5)): positionHeld = False break count = count + 1 self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") self.fpa.stop()
def test_attctl(self): # 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() self.rateSec.sleep() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") # set some attitude and thrust att = PoseStamped() att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) att.pose.orientation = Quaternion(*quaternion) throttle = Float64() throttle.data = 0.6 # does it cross expected boundaries in X seconds? count = 0 timeout = 120 while(count < timeout): # update timestamp for each published SP att.header.stamp = rospy.Time.now() self.pubAtt.publish(att) self.pubThr.publish(throttle) if (self.localPosition.pose.position.x > 5 and self.localPosition.pose.position.z > 5 and self.localPosition.pose.position.y < -5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to cross boundaries")