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