<reponame>zhang-hply/cf_develop
#!/usr/bin/env python

'''
1. takeoff
2. hover
6. land
'''

import math
import rospy
from crazyflie_driver.msg import Position
from std_msgs.msg import Empty
from geometry_msgs.msg import PoseStamped
from crazyflie_init import crazyflie_init

if __name__ == '__main__':

    rospy.init_node('hover', anonymous=True)

    if not crazyflie_init(rospy):
        rospy.logwarn(rospy.get_name()+" init failed !");
    exit(1)

    worldFrame = rospy.get_param("~worldFrame", "/world")
    topic = rospy.get_param("~topic", "external_pose")
    height = rospy.get_param("~height", 0.6)
    frequency = rospy.get_param("~frequency", 30)

    # pause 2s to wait for server starting
    pause = rospy.Rate(0.5)
    pause.sleep()

    rate = rospy.Rate(frequency)

    # record start point
    pose = rospy.wait_for_message(topic, PoseStamped)
    px_s = pose.pose.position.x
    py_s = pose.pose.position.y
    pz_s = pose.pose.position.z

    # position command
    pos_msg = Position()
    pos_msg.header.seq = 0
    pos_msg.header.stamp = rospy.Time.now()
    pos_msg.header.frame_id = worldFrame
    pos_msg.x = 0.0
    pos_msg.y = 0.0
    pos_msg.z = 0.0
    pos_msg.yaw = 0.0
    pos_pub = rospy.Publisher("cmd_position", Position, queue_size=1)

    stop_pub = rospy.Publisher("cmd_stop", Empty, queue_size=1)
    stop_msg = Empty()

    # take off
    rospy.loginfo(rospy.get_name() + " taking off ...")
    start = rospy.get_time()
    i = 0
    while not rospy.is_shutdown():
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.yaw = 0.0
        pos_msg.z = pz_s + i / 25.0
        i = i + 1
        now = rospy.get_time()
        if (now - start > 1.0):
            break
        pos_msg.header.seq += 1
        pos_msg.header.stamp = rospy.Time.now()
        pos_pub.publish(pos_msg)
        rate.sleep()

    rospy.loginfo(rospy.get_name() + " hovering ...")
    start = rospy.get_time()
    while not rospy.is_shutdown():
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.yaw = 0.0
        pos_msg.z = pz_s + height
        pos_msg.header.seq += 1
        now = rospy.get_time()
        if (now - start > 1.0):
            break
        pos_msg.header.stamp = rospy.Time.now()
        pos_pub.publish(pos_msg)
        rate.sleep()


    # land, spend 1 secs
    rospy.loginfo(rospy.get_name() + " landing ...")
    start = rospy.get_time()
    while not rospy.is_shutdown():
        t = now - start
        if (t > 1.2):
            break
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.z = height * (math.exp(-1.6 * t)) + pz_s
        pos_msg.yaw = 0.0
        now = rospy.get_time()
        pos_msg.header.seq += 1
        pos_msg.header.stamp = rospy.Time.now()
        pos_pub.publish(pos_msg)
        rate.sleep()

    stop_pub.publish(stop_msg)

#!/usr/bin/env python

'''
1. takeoff
2. hover
6. land
'''

import math
import rospy
from crazyflie_driver.msg import Position
from std_msgs.msg import Empty
from geometry_msgs.msg import PoseStamped
from crazyflie_init import crazyflie_init

if __name__ == '__main__':

    rospy.init_node('hover', anonymous=True)

    if not crazyflie_init(rospy):
        rospy.logwarn(rospy.get_name()+" init failed !");
    exit(1)

    worldFrame = rospy.get_param("~worldFrame", "/world")
    topic = rospy.get_param("~topic", "external_pose")
    height = rospy.get_param("~height", 0.6)
    frequency = rospy.get_param("~frequency", 30)

    # pause 2s to wait for server starting
    pause = rospy.Rate(0.5)
    pause.sleep()

    rate = rospy.Rate(frequency)

    # record start point
    pose = rospy.wait_for_message(topic, PoseStamped)
    px_s = pose.pose.position.x
    py_s = pose.pose.position.y
    pz_s = pose.pose.position.z

    # position command
    pos_msg = Position()
    pos_msg.header.seq = 0
    pos_msg.header.stamp = rospy.Time.now()
    pos_msg.header.frame_id = worldFrame
    pos_msg.x = 0.0
    pos_msg.y = 0.0
    pos_msg.z = 0.0
    pos_msg.yaw = 0.0
    pos_pub = rospy.Publisher("cmd_position", Position, queue_size=1)

    stop_pub = rospy.Publisher("cmd_stop", Empty, queue_size=1)
    stop_msg = Empty()

    # take off
    rospy.loginfo(rospy.get_name() + " taking off ...")
    start = rospy.get_time()
    i = 0
    while not rospy.is_shutdown():
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.yaw = 0.0
        pos_msg.z = pz_s + i / 25.0
        i = i + 1
        now = rospy.get_time()
        if (now - start > 1.0):
            break
        pos_msg.header.seq += 1
        pos_msg.header.stamp = rospy.Time.now()
        pos_pub.publish(pos_msg)
        rate.sleep()

    rospy.loginfo(rospy.get_name() + " hovering ...")
    start = rospy.get_time()
    while not rospy.is_shutdown():
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.yaw = 0.0
        pos_msg.z = pz_s + height
        pos_msg.header.seq += 1
        now = rospy.get_time()
        if (now - start > 1.0):
            break
        pos_msg.header.stamp = rospy.Time.now()
        pos_pub.publish(pos_msg)
        rate.sleep()


    # land, spend 1 secs
    rospy.loginfo(rospy.get_name() + " landing ...")
    start = rospy.get_time()
    while not rospy.is_shutdown():
        t = now - start
        if (t > 1.2):
            break
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.z = height * (math.exp(-1.6 * t)) + pz_s
        pos_msg.yaw = 0.0
        now = rospy.get_time()
        pos_msg.header.seq += 1
        pos_msg.header.stamp = rospy.Time.now()
        pos_pub.publish(pos_msg)
        rate.sleep()

    stop_pub.publish(stop_msg)
#!/usr/bin/env python 1. takeoff 2. hover 6. land # pause 2s to wait for server starting # record start point # position command # take off # land, spend 1 secs