ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Control - Marker and sequence is not correct

asked 2018-11-24 09:09:55 -0500

Dylan gravatar image

updated 2019-04-12 02:07:05 -0500

jayess gravatar image

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
            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"

        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 ...
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2018-11-25 06:09:19 -0500

updated 2018-11-26 04:45:01 -0500

The structure of this python node needs a bit of work.

You're creating a new instance of the gonnaGetDown object in every iteration of your main loop for starters so you creating and destroying the publisher and subscriber objects every time as well so your timing will be all over the place.

This is very important! The design ethos of ROS is that your publishers and subscribers are created once when your node starts up, they then run for the lifetime of the node, 99% of ROS nodes will work in this way.

To get this working you'll need do a few modification:

1) Move the creation of the gonnaGetDown object and the rospy.Rate object to before your while loop. They only need creating once, in fact rate won't work properly if you create a new once each time.

2) The while loop should be changed to while not rospy.is_shutdown(): instead of while(1):

3) The line velocity_publisher = rospy.Publisher('/quadrotor/cmd_vel', Twist, queue_size=10) needs to be in __init__ and should create a property so should be: self.velocity_publisher = rospy.Publisher('/quadrotor/cmd_vel', Twist, queue_size=10). They you can refer to it directly as self.velocity_publisher from then on instead of taking a copy of it each time.

4) Get rid of the cmdvel_publisher method, I don't what this is meant to be doing but it's a callback to a topic which is publishing on the same topic. This means that as soon as it receives a message it's stuck in an infinite loop of receiving and publishing on the same topic. The only reason this doesn't go on for ever in your code is that the object is destroyed at the end of each iteration of your main while loop.

5) You need to describe all of the behaviour of the cmd_vel topic within the detect_marker callback. Setting it to zero if no marker is detected and setting the z linear velocity to -0.01 if it is. You can also print out your debugging information here but note it will be printed once for each ar_pose_marker message received.

This should get your system behaving something like what you want.


To answer your additional questions:

You don't call objects you create them, this is a very important distinction. When the gonnaGetDown is created it's initializer creates a subscriber, from this point on when a messages is received on this topic it will automatically invoke code within the object. You do not need to do anything from this point for the object to do it's job.

Regarding the knowing the values of cmd_vel. Your own code is creating and publishing the values of cmd_vel, if you need to know what's it's values were in the past you can record them in a local variable within the object.

Can I recommend that you read up on event-driven programming here, this is the paradigm that ROS ... (more)

edit flag offensive delete link more


Thanks for you help. In (1), if I take rate = rospy.Rate(0.01) and gonna_get_down = gonnaGetDown() off the while not rospy.is_shutdown(), how the gonnaGetDown class would be called each time? or what do I have to put inside the while not rospy.is_shutdown()?

Dylan gravatar image Dylan  ( 2018-11-25 17:23:59 -0500 )edit

In (4), I tried to subscribe to the /cmd_vel topic because, when I have to use a PID control, I need to know what is inside the /cmd_vel topic. So I need to subscribe to it. How do I do that subscription if I don't do it that way?

Dylan gravatar image Dylan  ( 2018-11-25 17:29:34 -0500 )edit

I'll answer these queries in my original reply.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-11-26 04:34:22 -0500 )edit

Question Tools

1 follower


Asked: 2018-11-24 09:09:55 -0500

Seen: 359 times

Last updated: Apr 12 '19