Question

Hi,

I am trying to write a subscriber which subscribes to the /cf<id>/pose topic (of the Crazyswarm ROS package) to retrieve the pose of my Crazyflie quadcopter, where <id> is the index of the Crazyflie, which is 1 in my case since I am testing it with only one quadcopter for now. However, only the variable initialization (np.ones(3); np.ones(4)) gets returned and not the updated value from the callback function. I have tried checking the contents of the topic's message with rostopic echo /cf<id>/pose, and seem to be getting the correct real-time position of the robot. I have also tried using the global keyword within the callback function with no luck. Plus, initializing p and theta with None returns the error:

TypeError: 'NoneType' object is not subscriptable.

What could I be doing wrong? I have pored over robotics.stackexchange and ROS Answers and tried every solution I could find, and am still getting the same results, so there is definitely something wrong with my code.

Here's my code:

import numpy as np
import math
import rospy
from geometry_msgs.msg import Pose
import tf

p = np.ones(3)
theta = np.ones(4)

def allPoseCallback(data):
    global p
    global theta

    p = np.array(data.position)
    theta = np.array(data.orientation)


def cfPoseSubscriber():
    rospy.Subscriber('/cf1/pose', Pose, allPoseCallback)

allcfpose = {} #Initialize empty dictionary
allcfrpy = np.zeros((1,3)) #Array to hold roll pitch yaw values of one CF

if __name__ == '__main__':
        rospy.init_node('cflistener', anonymous=True)  

        k = 0

        while not rospy.is_shutdown():
            cfPoseSubscriber()
            allcfrpy[k,:] = np.array(tf.transformations.euler_from_quaternion(theta)) #Populate allcfrpy using rpy angles
            allcfpose[f'cf{k+1}'] = np.array(p) #Populate dictionary using pose data

                # k+=1

            print(allcfpose)
            print('\n----------------------------------\n')
            print(allcfrpy)

Any help will be appreciated. Many thanks.

Asked by cenwerem on 2022-06-23 14:39:51 UTC

Answer 1

Your while loop has problems:

Do not keep re-subscribing every time through the loop. Do it once before you enter the loop.
You must sleep() inside the while loop. Use rospy.sleep(n), or use a rospy.Rate object.

To get started, try n = 0.250

Asked by Mike Scheutzow on 2022-06-23 19:15:36 UTC

Comments

Thank you for your response. I have effected the changes you highlighted, but I am still getting the same behavior; higher or lower values of the sleep period also do not change the result. I have also tried using empty arrays and can confirm that the callback isn't being called, because I get the empty arrays back exactly. I should also add that defining the variables, subscriber, and callback within a class yields similar results. Perhaps, there is some other way of initializing the p and theta variables other than None or empty arrays that just might work. Thanks for your time and help.

Asked by cenwerem on 2022-06-23 22:15:55 UTC

I could not get this to work, but in the meantime, I've had to use the position() method of the Crazyflie class, which returns the real-time position of the Crazyflie. There isn't one for the attitude, but I only need the position currently and can just assume that all the robots are in a hover state for now.

Asked by cenwerem on 2022-06-24 10:47:30 UTC

If you add a try/except around the code in your subscribe callback, and print any exception that is thrown, you will likely discover what is wrong. I'm skeptical that np.array(data.position) is valid code.

Asked by Mike Scheutzow on 2022-06-24 10:59:43 UTC

Yeah, I did take out the np.array part in a later version of the code and left it just as data.position, but still got empty values. Thanks, I didn't think of the try/except part. I'll add a try/except block in my callback to see if I can catch the error. Thank you.

Asked by cenwerem on 2022-06-24 18:38:32 UTC

I have this in the callback now, but the except block doesn't print anything. Is this implementation right? Thanks.

p = []
theta = []
def allPoseCallback(data):
    try:
        global p
        global theta

        p = data.position
        theta = data.orientation
    except len(p) == 0:
        print("Callback didn't run!")

Asked by cenwerem on 2022-06-24 18:44:02 UTC

No, not right: position is not an array. data is a geometry_msgs/Pose object, so position is a geometry_msgs/Position object. You can use rosmsg terminal command to quickly see the structure:

rosmsg info geometry_msgs/Pose

Also, the except keyword has to be followed by an Exception class type. Do an internet search for this.

Asked by Mike Scheutzow on 2022-06-25 08:32:06 UTC

Got it. Thanks.

Asked by cenwerem on 2022-06-25 10:17:34 UTC

I just wanted to add that the problem was because I was listening for the wrong message type in my subscriber function. The message type should have been PoseStamped and not Pose. Thanks to everyone for the help!

Asked by cenwerem on 2022-07-10 18:36:54 UTC

ROS Subscriber not updating values with callback data

Comments

Answers

Comments