Robotics StackExchange | Archived questions

How does subscriber works?

In the code below, turtlebot is moved using /turtle1/cmdvel topic and we get current pose using /turtle1/Pose topic. My question is if we see the code in if _name__ == 'main': block, we initialise node, declare subscriber and call for publisher function i.e. motion(speed,distance). This code works perfectly fine. I would like to know how does it get current pose values all the time, how is this program executed? Is it like prem-emptive scheduler, something like RTOS systems, just a thought. I would like to know the background logic and working here.

#!/usr/bin/env python

#libaries

import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
import math
import time
from std_srvs.srv import Empty

#initialize variables
x = 0
y = 0
z = 0
yaw = 0

#call back function for subscriber

def pose_callback(posedata):
    global x,y,z,yaw
    x = posedata.x
    y = posedata.y
    yaw = posedata.theta

    #print("Pose CallBack\nx={}\ny={}\nyaw={}".format(posedata.x,posedata.y,posedata.theta))

# publisher function
def motion(speed,distance):
    vel_msg = Twist()

    # get current location
    x0 = x
    y0 = y

    # set velocities to speed
    vel_msg.linear.x = speed

    distance_moved = 0
    loop_rate = rospy.Rate(10)

    # define publisher
    vel_publisher = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size = 10)

    while not rospy.is_shutdown():
        rospy.loginfo("Turtlesim Moves Forward")

        #publish topic = writing to variable
        vel_publisher.publish(vel_msg)

        loop_rate.sleep()

        distance_moved = distance_moved + math.sqrt((x-x0)**2 + (y-y0)**2)
        rospy.loginfo(distance_moved)
        print(distance_moved)

        if not(distance_moved<distance):
            print("Reahed")
            break


    vel_msg.linear.x = 0
    vel_msg.linear.y = 0
    vel_publisher.publish(vel_msg)




if __name__ == '__main__':
    try:
        # intialise node, node name
        rospy.init_node("my_turtlesim_pose",anonymous=True)

        #call subscriber - read values
        rospy.Subscriber("/turtle1/pose",Pose,pose_callback)
        time.sleep(2)
        print ('move: ')

        #write values
        motion (5.0, 5.0)

        time.sleep(2)
        print ('start reset: ')
        rospy.wait_for_service('reset')
        reset_turtle = rospy.ServiceProxy('reset', Empty)
        reset_turtle()
        print ('end reset: ')
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Node Terminated")

Asked by ratz on 2019-12-30 15:54:56 UTC

Comments

Please edit your question to provide enough context to reproduce your problem. It's not clear what you mean "we don't call it in publish" and what other things are running on your system? Please take a look at the question guidelines at: https://wiki.ros.org/Support

Asked by tfoote on 2019-12-30 17:48:09 UTC

Answers

ROS creates separate thread for each subscriber. That is why you are getting pose values updated. You can find more detailed explanation and examples in here: https://answers.ros.org/question/9543/rospy-threading-model/

Asked by Nikodem on 2019-12-31 08:18:34 UTC

Comments

From the link you shared:

For a particular topic, there is only one thread that all the subscribers share.

I think the question was aimed more at asking how callbacks work.

Asked by allenh1 on 2019-12-31 10:55:05 UTC

if name == 'main': try: rospy.init_node('turtlesim_motion_pose', anonymous=True)

    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)


    position_topic = "/turtle1/pose"
    pose_subscriber = rospy.Subscriber(position_topic, Pose, poseCallback)
    time.sleep(2)
    print ('move: ')
    move (1.0, 5.0)
    time.sleep(2)
    print ('start reset: ')
    rospy.wait_for_service('reset')
    reset_turtle = rospy.ServiceProxy('reset', Empty)
    reset_turtle()
    print ('end reset: ')
    rospy.spin()
except rospy.ROSInterruptException:
    rospy.loginfo("node terminated.")

Asked by Beam on 2021-07-12 12:37:52 UTC

Comments