#!/usr/bin/env python

'''
go circle
1. takeoff @ 0.5m
2. hover
3. move to x=radius y=0
4. draw a cicle counterclockwise
5. move to start point
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_driver.msg import VelocityWorld
from crazyflie_init import crazyflie_init

if __name__ == '__main__':

    rospy.init_node('position', 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")
    radius = rospy.get_param("~radius", 0.6)
    velocity = rospy.get_param("~velocity", 0.4)
    height = rospy.get_param("~height", 0.6)
    kPositionCompensation = rospy.get_param("~kPositionCompensation", 2)
    frequency = rospy.get_param("~frequency", 30)

    kOverflow = 4

    # 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

    # velocity command
    vel_msg = VelocityWorld()
    vel_msg.header.seq = 0
    vel_msg.header.stamp = rospy.Time.now()
    vel_msg.header.frame_id = worldFrame
    vel_msg.vel.x = 0
    vel_msg.vel.y = 0
    vel_msg.vel.z = 0
    vel_pub = rospy.Publisher("cmd_velocity_world", VelocityWorld, queue_size=1)

    # 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)
    # for logging usage
    pos_log_pub = rospy.Publisher("cmd_position_log", 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()

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

        # circle
    rospy.loginfo(rospy.get_name() + " drawing circle ...")
    startTime = rospy.get_time()
    center_x = px_s
    center_y = py_s

    while not rospy.is_shutdown():

        pose = rospy.wait_for_message(topic, PoseStamped)
        px = pose.pose.position.x
        py = pose.pose.position.y
        pz = pose.pose.position.z

        t = rospy.get_time() - startTime
        if t > 1.1 * 2 * math.pi * radius / velocity and (px - radius - px_s) * (px - radius - px_s) + (py - py_s) * (
                py - py_s) < 0.15:
            break

        px_d = radius * math.cos(velocity / radius * t) + px_s
        py_d = radius * math.sin(velocity / radius * t) + py_s
        vx_d = -velocity * math.sin(velocity / radius * t)
        vy_d = velocity * math.cos(velocity / radius * t)

        vel_msg.header.stamp = rospy.Time.now()
        vel_msg.header.seq += 1
        vel_msg.vel.x = vx_d + kPositionCompensation * (px_d - px)
        vel_msg.vel.y = vy_d + kPositionCompensation * (py_d - py)
        vel_msg.vel.z = kPositionCompensation * (height - pz)

        if vel_msg.vel.x * vel_msg.vel.x + vel_msg.vel.y * vel_msg.vel.y + vel_msg.vel.z * vel_msg.vel.z > velocity * velocity * kOverflow * kOverflow:
            rospy.logerr(rospy.get_name() + " velocity overflow")
            break

        vel_pub.publish(vel_msg)

        # log
        pos_msg.x = px_d
        pos_msg.y = py_d
        pos_msg.yaw = 0.0
        pos_msg.z = height
        pos_msg.header.seq += 1
        pos_msg.header.stamp = rospy.Time.now()
        pos_log_pub.publish(pos_msg)

        rate.sleep()

    rospy.loginfo(rospy.get_name() + " moving back ...")
    start = rospy.get_time()
    while not rospy.is_shutdown():
        now = rospy.get_time()
        if (now - start > 3.0):
            break
        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
        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():
        now = rospy.get_time()
        t = now - start
        if (t > 1.5):
            break
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.z = height * (math.exp(-1.4 * 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

'''
go circle
1. takeoff @ 0.5m
2. hover
3. move to x=radius y=0
4. draw a cicle counterclockwise
5. move to start point
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_driver.msg import VelocityWorld
from crazyflie_init import crazyflie_init

if __name__ == '__main__':

    rospy.init_node('position', 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")
    radius = rospy.get_param("~radius", 0.6)
    velocity = rospy.get_param("~velocity", 0.4)
    height = rospy.get_param("~height", 0.6)
    kPositionCompensation = rospy.get_param("~kPositionCompensation", 2)
    frequency = rospy.get_param("~frequency", 30)

    kOverflow = 4

    # 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

    # velocity command
    vel_msg = VelocityWorld()
    vel_msg.header.seq = 0
    vel_msg.header.stamp = rospy.Time.now()
    vel_msg.header.frame_id = worldFrame
    vel_msg.vel.x = 0
    vel_msg.vel.y = 0
    vel_msg.vel.z = 0
    vel_pub = rospy.Publisher("cmd_velocity_world", VelocityWorld, queue_size=1)

    # 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)
    # for logging usage
    pos_log_pub = rospy.Publisher("cmd_position_log", 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()

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

        # circle
    rospy.loginfo(rospy.get_name() + " drawing circle ...")
    startTime = rospy.get_time()
    center_x = px_s
    center_y = py_s

    while not rospy.is_shutdown():

        pose = rospy.wait_for_message(topic, PoseStamped)
        px = pose.pose.position.x
        py = pose.pose.position.y
        pz = pose.pose.position.z

        t = rospy.get_time() - startTime
        if t > 1.1 * 2 * math.pi * radius / velocity and (px - radius - px_s) * (px - radius - px_s) + (py - py_s) * (
                py - py_s) < 0.15:
            break

        px_d = radius * math.cos(velocity / radius * t) + px_s
        py_d = radius * math.sin(velocity / radius * t) + py_s
        vx_d = -velocity * math.sin(velocity / radius * t)
        vy_d = velocity * math.cos(velocity / radius * t)

        vel_msg.header.stamp = rospy.Time.now()
        vel_msg.header.seq += 1
        vel_msg.vel.x = vx_d + kPositionCompensation * (px_d - px)
        vel_msg.vel.y = vy_d + kPositionCompensation * (py_d - py)
        vel_msg.vel.z = kPositionCompensation * (height - pz)

        if vel_msg.vel.x * vel_msg.vel.x + vel_msg.vel.y * vel_msg.vel.y + vel_msg.vel.z * vel_msg.vel.z > velocity * velocity * kOverflow * kOverflow:
            rospy.logerr(rospy.get_name() + " velocity overflow")
            break

        vel_pub.publish(vel_msg)

        # log
        pos_msg.x = px_d
        pos_msg.y = py_d
        pos_msg.yaw = 0.0
        pos_msg.z = height
        pos_msg.header.seq += 1
        pos_msg.header.stamp = rospy.Time.now()
        pos_log_pub.publish(pos_msg)

        rate.sleep()

    rospy.loginfo(rospy.get_name() + " moving back ...")
    start = rospy.get_time()
    while not rospy.is_shutdown():
        now = rospy.get_time()
        if (now - start > 3.0):
            break
        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
        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():
        now = rospy.get_time()
        t = now - start
        if (t > 1.5):
            break
        pos_msg.x = px_s
        pos_msg.y = py_s
        pos_msg.z = height * (math.exp(-1.4 * 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 go circle 1. takeoff @ 0.5m 2. hover 3. move to x=radius y=0 4. draw a cicle counterclockwise 5. move to start point 6. land # pause 2s to wait for server starting # record start point # velocity command # position command # for logging usage # take off # circle # log # land, spend 1 secs