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_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")