modrone

#!/usr/bin/env python

import math
import rospy
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import PoseStamped
from crazyflie_driver.msg import Position

# Current goal (global state)
goal = None

def goal_callback(msg):
    global goal

    # RViz's "2D Nav Goal" publishes z=0, so add some altitude if needed.
    if msg.pose.position.z == 0.0:
        msg.pose.position.z = 0.4

    rospy.loginfo('New goal set:\n%s', msg)
    goal = msg

def publish_cmd(goal):
    cmd = Position()
    cmd header.stamp = rospy.Time.now()
    cmd.header.frame_id = 'map'
    cmd.x = m['pose']['position'][0]
    cmd.y = m['pose']['position'][1]
    cmd.z =  0.3
    cmd.yaw = m['pose']['orientation'][2]
    
    pub_cmd.publish(cmd)

def dist(myposex,myposey,markx,marky):
    d = math.sqrt((myposex-markx)**2+(myposey-marky)**2)
    return d
    
def new_goal(mypose):
    global origin, marker0, marker 9, state, stopsign
    tol = 0.1
    if dist(mypose.pose.position.x,mypose.pose.position.y,
    marker0 ['pose']['position'][0].marker0 ['pose']['position'][1]) < tol:
        state = 2
    elif dist(mypose.pose.position.x,mypose.pose.position.y,
    marker9 ['pose']['position'][0],marker9['pose']['position'][1]) < tol:
        state = 1
    elif dist(mypose.pose.position.x,mypose.pose.position.y,
    stopsign ['pose']['position'][0],stopsign['pose']['position'][1]) < tol:
        state = 2

rospy.init_node('movedrone')
sub_pose  = rospy.Subscriber('/cf1/pose', Posestamped, new_goal)
pub_cmd = rospy.Publisher('/cf1/cmd_position', Position, queue_size = 2)
tf_buf = tf2_ros.Buffer()
tf_lstn =  tf2_ros.TransformListener(tf_buf)


def main(argv = sys.argv):
    global origin, marker0, marker9, state, stopsign
    rate = rospy.Rate(10)  # Hz

    args = rospy.myarv(argv = argv)

    with open(args[1],'rb') as f:
        world = json.load(f)

    markers = world['markers']
    origin = world['origin'][0]
    marker0 = markers[0]
    marker0['pose']['orientation'][2] +=90
    marker9 = markers[9]
    stopsign = world['roadsigns'][14]
    stopsign['pose']['position'][1] -= 0.4

    while not rospy.is_shutdown():
        if state == 0:
            publish_cmd(marker9)
        elif state == 1:
            publish_cmd(marker0)
        elif state == 2:
            publish_cmd(stopsign)    
        rate.sleep()

if __name__ == '__main__':
    main()