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