Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

How to feed the data obtained from rospy.Subscriber data into a variable?

Tags:

python

ros

rospy

I have written a sample Subscriber. I want to feed the data that I have obtained from the rospy.Subscriber into another variable, so that I can use it later in the program for processing. At the moment I could see that the Subscriber is functioning as I can see the subscribed values being printed when I use rospy.loginfo() function. Although I donot know how to store this data into another varible. I have tried assigning it directly to a variable by using assignment operator '=', but I get error.

I have tried writing a callback function with rospy.loginfo to print the position data from the subscribed object. I have subscribed JointState and it containes, header, position, velocity and effort arrays. using rospy.loginfo I can verify that the subscriber is subscribing. But when i tried to assign it directly to a variable, I get an error.

I am displaying loginfo from a call back function as follows

def callback(data):
   rospy.loginfo(data.position)
   global listen
    listen = rospy.Subscriber("joint_states", JointState, 
    callback)
    rospy.spin()

and this works fine. But when i slightly modify the code to assign the subscribed values, I get following error i.e.

   listen1 = rospy.Subscriber("joint_states", JointState, 
   callback=None)
   listen = listen1.position
   #rospy.loginfo(listen)
   print(listen)
   rospy.spin()```

The error is as follows, 
 ```listen = listen1.position
    AttributeError: 'Subscriber' object has no attribute 'position'

EDIT: Here is my node I have defined in my program,

    #rospy.loginfo(msg.data)
    global tactile_states
    tactile_states = data.data

def joint_callback(data):
    #rospy.loginfo(data.position)
    global g_joint_states 
    global g_position
    global g_pos1
    g_joint_states = data
    #for i in len(data.position):
        #g_position[i] = data.position[i]
    g_position = data.position
    if len(data.position) > 0:
        print("jointstate more than 0")
        g_pos1 = data.position[0]
    #print(g_position)


def joint_modifier(*args):
    #choice describes what the node is supposed to do whether act as publisher or subscribe to joint states or tactile sensors
    rospy.init_node('joint_listener_publisher', anonymous=True)
    pub1 = rospy.Publisher('joint_states', JointState, queue_size = 10)
    if(len(args)>1):
        choice = args[0]
        joint_name = args[1]
        position = args[2]
    else:
        choice = args[0]
    if (choice == 1):
        rate = rospy.Rate(1)
        robot_configuration = JointState()
        robot_configuration.header = Header()
        robot_configuration.name = [joint_name]
        robot_configuration.position = [position]
        robot_configuration.velocity = [10]
        robot_configuration.effort = [100]
        while not rospy.is_shutdown():
            robot_configuration.header.stamp = rospy.Time.now()
            rospy.loginfo(robot_configuration)
            break
        pub1.publish(robot_configuration)
        rospy.sleep(2)
    if (choice == 2):
        #rospy.Timer(rospy.Duration(2), joint_modifier)
        listen = rospy.Subscriber("joint_states", JointState, joint_callback)
        rospy.spin()
    if (choice == 3):
        #rospy.Timer(rospy.Duration(2), joint_modifier)
        tactile_sub = rospy.Subscriber("/sr_tactile/touch/ff", Float64, tactile_callback)
        rospy.spin()

This is how I am calling the node inside the main body of the program,

           joint_modifier(2)
           print("printing g_position")
           print(g_position)#to check the format of g_position
           print("printed g _position")
           leg_1 = Leg_attribute(g_position[0], g_position[1], g_position[2], velocity1 = 10, velocity2 = 10, velocity3 = 10, effort1 = 100, effort2 = 100, effort3 = 100, acceleration=1)

When calling this way, the program is stuck at joint_modifier(2) as that function has rospy.spin().


like image 582
Sai Raghava Avatar asked Oct 27 '22 10:10

Sai Raghava


1 Answers

The style which you're using is not very standard. I assume you've seen the example on ROS wiki, I've modified it to demonstrate standard usage below.

Chiefly, addressing the code you posted, you needed to make listen have global scope outside of the callback. This is to store the data you want, not the Subscriber object. The rospy.spin() never goes in a callback, only the main node function/section. The subscriber object, listen1, which is used infrequently, doesn't return anything, and doesn't store the data it acquires. That is, you need Subscriber() to have a non-None callback. It's more of a bind, giving the data to the callback instead of returning it from Subscriber. That's why listen1 (Subscriber) has no attribute position (JointState).

import rospy
from sensor_msgs.msg import JointState

# Subscribers
#     joint_sub (sensor_msgs/JointState): "joint_states"

# This is where you store all your data you recieve
g_joint_states = None
g_positions = None
g_pos1 = None

def timer_callback(event): # Type rospy.TimerEvent
    print('timer_cb (' + str(event.current_real) + '): g_positions is')
    print(str(None) if g_positions is None else str(g_positions))

def joint_callback(data): # data of type JointState
    # Each subscriber gets 1 callback, and the callback either
    # stores information and/or computes something and/or publishes
    # It _does not!_ return anything
    global g_joint_states, g_positions, g_pos1
    rospy.loginfo(data.position)
    g_joint_states = data
    g_positions = data.position
    if len(data.position) > 0:
        g_pos1 = data.position[0]
    print(g_positions)

# In your main function, only! here do you subscribe to topics
def joint_logger_node():
    # Init ROS
    rospy.init_node('joint_logger_node', anonymous=True)

    # Subscribers
    # Each subscriber has the topic, topic type, AND the callback!
    rospy.Subscriber('joint_states', JointState, joint_callback)
    # Rarely need to hold onto the object with a variable: 
    #     joint_sub = rospy.Subscriber(...)
    rospy.Timer(rospy.Duration(2), timer_callback)

    # spin() simply keeps python from exiting until this node is stopped
    # This is an infinite loop, the only code that gets ran are callbacks
    rospy.spin()
    # NO CODE GOES AFTER THIS, NONE! USE TIMER CALLBACKS!
    # unless you need to clean up resource allocation, close(), etc when program dies

if __name__ == '__main__':
    joint_logger_node()

Edit 1: There seems to be some confusion on what Subscriber(), spin(), and _callback(s) do. It's a bit obscured in the Python, but there is a master program that manages all nodes, and sending nodes between them. In each node, we register with that master program that the node exists, and what publishers and subscribers it has. By register, it means we tell the master program, "Hey, I want that topic!"; in your case, for your (undeclared) joint_sub Subscriber, "Hey, I want all the JointState msgs from the joint_states topic!" The master program will, every time it gets (from some publisher somewhere) a new joint_states JointState msg, send it to that subscriber. The subscriber handles, deals with, and processes the msg (data) with a callback: when(!) I receive a message, run the callback.

So the master program receives a new joint_states JointState msg from some publisher. Then it, because we registered a subscriber to it, sends it to this node. rospy.spin() is an infinite loop waiting for that data. This is what it does (kinda-mostly):

def rospy.spin():
    while rospy.ok():
        for new_msg in get_new_messages from master():
            if I have a subscriber to new_msg:
                my_subscriber.callback(new_msg)

rospy.spin() is where your callback, joint_callback (and/or timer_callback, etc) actually get called, and executed. It only runs when there is data for it.

More fundamentally, I think because of this confusion, your program structure is flawed; your functions don't do what you think they do. This is how you should make your node.

  1. Make your math-portion (all the real non-ros code), the one doing the NN, into a separate module, and make a function to run it.
  2. If you only want to run it when you receive data, run it in the callback. If you want to publish the result, publish in the callback.
  3. Don't call the main function! The if __name__ == '__main__': my_main_function() should be the only place it gets called, and this will call your code. I repeat: the main function, declaring subscribers/publishers/init/timers/parameters, is only run in if __name__ ..., and this function runs your code. To have it run your code, place your code in a callback. Timer callbacks are handy for this.

I hope this code sample clarifies:

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import JointState
import my_nn as nn # nn.run(data)

# Subscribers
#     joint_sub (sensor_msgs/JointState): "joint_states"

# Publishers
#     joint_pub (sensor_msgs/JointState): "target_joint_states"

joint_pub = None

def joint_callback(data): # data of type JointState
    pub_msg = JointState() # Make a new msg to publish results
    pub_msg.header = Header()
    pub_msg.name = data.name
    pub_msg.velocity = [10] * len(data.name)
    pub_msg.effort = [100] * len(data.name)
    # This next line might not be quite right for what you want to do,
    # But basically, run the "real code" on the data, and get the
    # result to publish back out
    pub_msg.position = nn.run(data.position) # Run NN on data, store results
    joint_pub.publish(pub_msg) # Send it when ready!

if __name__ == '__main__':
    # Init ROS
    rospy.init_node('joint_logger_node', anonymous=True)
    # Subscribers
    rospy.Subscriber('joint_states', JointState, joint_callback)
    # Publishers
    joint_pub = rospy.Publisher('target_joint_states', JointState, queue_size = 10)
    # Spin
    rospy.spin()
    # No more code! This is not a function to call, but its
    # own program! This is an executable! Run your code in
    # a callback!

Notice that a python module we design to be a ros node, has no functions to be called. It has a defined structure of callbacks and global data shared between them, all initialized and registered in the main function / if __name__ == '__main__'.

like image 124
JWCS Avatar answered Nov 15 '22 06:11

JWCS