tune_altitude.py

#!/usr/bin/env python
import rospy
from rosplane_msgs.msg import Controller_Commands
import std_msgs
from math import pi

def main():
    rospy.init_node('altitude_tuner', anonymous=True)
    command_topic = rospy.get_param('~command_topic', 'controller_commands')
    command_airspeed = rospy.get_param('~airspeed', 30.0)
    center_altitude = rospy.get_param('~center_altitude',180.0)
    amplitude = rospy.get_param('~amplitude', 4.0)
    period = rospy.get_param('~period', 20)
    command_course = rospy.get_param('~course', pi/2)

    command_pub = rospy.Publisher(command_topic, Controller_Commands, queue_size=1)
    neg_alt_pub = rospy.Publisher('negative_altitude_command', std_msgs.msg.Float32, queue_size = 1)

    command = Controller_Commands()
    command.Va_c = command_airspeed
    command.chi_c = command_course
    command.aux_valid = False

    neg_alt_msg = std_msgs.msg.Float32()

    rate = rospy.Rate(2./period)
    up = True
    while not rospy.is_shutdown():
        if up:
            command.h_c = center_altitude + amplitude/2
        else:
            command.h_c = center_altitude - amplitude/2
        neg_alt_msg.data = -command.h_c
        command_pub.publish(command)
        neg_alt_pub.publish(neg_alt_msg)
        up = not up
        print("pub", command.h_c)
        rate.sleep()

if __name__=='__main__':
    main()