class PreserveHeight:
def __init__(self, height):
self.pidInput = rospy.Publisher('pid_input',
controller_msg,
queue_size=10)
self.height = height
rospy.Subscriber('/pid/controller_command', controller_msg,
self.HandlePID)
rospy.Subscriber('/mavros/global_position/rel_alt', Float64,
self.HandleAltitude)
self.myNavigator = Navigator()
def StartMission(self):
print("Beginning Preserve Height Mission")
print("Arming")
if self.myNavigator.Arm(True):
print("Armed")
time.sleep(1)
def HandlePID(self, data):
self.myNavigator.PublishRCMessage(1500, 1500, data.z, 1500)
print("PID Fix %s" % data.z)
#Callback method for the rel_altitude topic
def HandleAltitude(self, data):
msg = controller_msg()
msg.z = self.height - data.data #only working on alt values for now
self.pidInput.publish(msg)
