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

前端 未结 1 645
醉梦人生
醉梦人生 2021-01-06 11:41

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 f

相关标签:
1条回答
  • 2021-01-06 12:29

    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 __init__ == '__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 __init__ ..., 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 __init__.

    0 讨论(0)
提交回复
热议问题