Feedback loop for a quadcopter (AR.Drone) [closed]

edit

I'm trying to make the feedback loop for my Gazebo simulation and I'm a bit lost. I want to subscribe to a topic that has the information of the distance (x, y, z) and publish other information (based on what I read) in the /cmd_vel topic.

The topic I would like to subscribe is /visualization_marker. If it is not possible, I can also subscribe to /tf and read just the specific frame I need.

The programming language would be Python, because I understand it better than C++. I tried to make a simple node based on the ROS tutorial for a publisher and the teleop_twist_keyboard node, that applies some velocities to the /cmd_vel topic, but it does nothing, and I think it's because there are some "tricks" for the AR.Drone that I'm not considering.

The camera that I would always use is the bottom one.

This is the code I wrote:

#!/usr/bin/env python

## Simple talker demo that published std_msgs/Strings messages to the 'chatter' topic

import rospy # you need to import rospy if you are writing a ROS Node
from std_msgs.msg import String #so we can reuse the std_msgs/String message type (as a simple string container) for publishing 

from geometry_msgs.msg import Twist

import termios, tty

#def talker():


if __name__ == '__main__':

    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
    rospy.init_node('talker', anonymous=True) #nombre del nodo. anonymous = True ensures that your node has a unique name by adding random numbers to the end of NAME

    speed = rospy.get_param("~speed", 0.5)
    turn = rospy.get_param("~turn", 1.0)

    x = 0
    y = 0
    z = 0


    try:
        rate = rospy.Rate(10) # 10hz - we should expect to go through the loop 10 times per second
        while not rospy.is_shutdown(): #check if your program should exit (e.g. if there is a Ctrl-C or otherwise)
            x = 1
            y = 1
            z = 0
        th = 1
            rate.sleep() #sleeps just long enough to maintain the desired rate through the loop

            twist = Twist()
            twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
            pub.publish(twist)

    except rospy.ROSInterruptException: #so that you don't accidentally continue executing code after the sleep()
        pass

So, my questions are:

• How can I make that feedback loop possible?

• How can I read a specific frame of a topic (/tf or /visualization_marker) and, based on what is read, publish in /cmd_vel?

Thanks :D