Control - Marker and sequence is not correct

edit

Hello everyone. I'm using an AR.Drone 2.0 and now I'm trying to design a simple PID to move the drone to the desired position. But I have a previuos error that is ruining my life:

• when I use the teleop_twist_keyboard and make the drone move slowly down, the z position of the drone using print "setpoint: %f \n" % marker.pose.pose.position.z is printed perfectly.

• now, when I make the drone move at the same velocity but using a Python program, it works in a bad way. There are 2 principal problems: first, the sequence printed is something like: a-b-dd-cccccc-d-a-b-dd-ccccccc-a-b-dd-cccccc-... (the code is below), and ideally I would like a-b-c-d-a-b-c-d-a-b-c-d-..., and the second problem is that the position is printed like it is in a loop, but the drone is always going down, like: 1.7889-1.7765-1.7632-1.7521-1.7476-1.7387-1.7265-1.7211-1.7165-1.7098-1.7003-1.6954-1.6898-1.6810-1.6743-1.7889-1.7765-... (I mean, it always returns to a loop)

The way that I run the program is:

1) lunch an empty world and place an ARTag

2) takeoff the drone and move it up using the teleop_twist_keyboard

3) lunching a detector for the marker

4) run the Python program. To make the program reach the "c" and "d", I have to press a button (anyone) in the teleop_twist_keyboard

I suppose that both problems are something about the rate, because it prints a lot of "d" and positions when it's working, but I don't know where to reduce it. I already reduced the robot_state_publisher to 1 Hertz and it's working much faster.

This is the code:

#!/usr/bin/env python
import rospy
import sys
from std_msgs.msg import String
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from ardrone_autonomy.msg import  Navdata
import threading
import tf
import time
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Image, CameraInfo, CompressedImage
from cv_bridge import CvBridge, CvBridgeError
import cv2
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
import numpy as np
from time import sleep
from states_variables.ARdroneStateVariables import ARdroneStateVariables
from ar_track_alvar_msgs.msg import AlvarMarkers


class gonnaGetDown():
    velocity_publisher = rospy.Publisher('/quadrotor/cmd_vel', Twist, queue_size=10)

    def __init__(self):    
        print "b"

        rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.detect_marker)

    def detect_marker(self, msg):
        print "c"
        if not msg.markers:
            print "Tag not found"
            vel_msg = Twist()

            vel_msg.linear.x = 0.0
            vel_msg.linear.y = 0.0
            vel_msg.linear.z = 0.0
            vel_msg.angular.x = 0.0
            vel_msg.angular.y = 0.0
            vel_msg.angular.z = 0.0

            velocity_publisher = gonnaGetDown.velocity_publisher
            velocity_publisher.publish(vel_msg)
            return
        else:
            marker = msg.markers[0]
            rospy.Subscriber('/quadrotor/cmd_vel', Twist, self.cmdvel_publisher, marker)           

    def cmdvel_publisher(self, msg, marker): #msg es de tipo Twist porque lo pase asi en la suscripcion de arriba
        print "d"
        sleep(0.2)

        velocity_publisher = gonnaGetDown.velocity_publisher

        print "z position: %f \n" % marker.pose.pose.position.z

        velocidad_z = -0.01
        vel_msg = Twist()

        vel_msg.linear.z = velocidad_z ...