I have this code which receives data from node (subscribe) and then inside this node I do some mathematical operation, then I want to publish the data to a certain topic.
#!/usr/bin/env python
import rospy
from std_msgs.msg import String, Float32
from math import cos,sin
import sys
import os
import logging
from types import *
import time
class exact_pose():
def callback_car_yaw(self, data):
self.car_yaw = data
def callback_line_pose(self,data):
self.line_pose = data
def init(self):
self.car_yaw = None
self.line_pose = None
def subscriber(self):
self.car_yaw_sub = rospy.Subscriber('~car/yaw',Float32, self.callback_car_yaw)
self.line_pose_sub = rospy.Subscriber('~line_pose',Float32, self.callback_line_detect_xL15)
def publisher(self):
self.exact_pose_pub = rospy.Publisher('exact_pose',Float32, queue_size=10)
rospy.Rate(1)
rospy.spin()
#sloving for exact pose
def calculation(self,):
while not rospy.is_shutdown():
self.exact_pose = self.line_pose_sub * cos(self.car_yaw_sub) + self.line_pose_sub * sin(self.car_yaw_sub)
info1 = Float32()
info1.data = self.exact_pose
self.exact_pose_pub.publish(info1)
rospy.Rate.sleep()
rospy.spin()
if __name__ == '__main__':
rospy.init_node('exact_pose', anonymous=True)
try:
ne = exact_pose()
except rospy.ROSInterruptException: pass
so what the code does, subscribe to a topic from a previous node, then do some mathematical operation for these data inside the topics, then publish the last result on separate Node
When requiring asynchronous communication, like in subscribers, multiple threads running in parallel will have to be involved. ROS uses callbacks in order to hide multi-threading required for subscribers from its users. When a new message arrives these callbacks will be added to a callback queue (whose maximum size can be defined for each publisher/subscriber) and then be processed. The way this works differs between C++ and Python:
ros::spin() once or repeatedly calling ros::spinOnce(). The processing of these callback queue happens sequentially by means of a spinner thread but you can deploy multiple spinner threads to increase the throughput as well as use different node handles that process different subscribers. A more detailed explanation can be found here.rospy.spin() does is block the main thread from returning and thus keep the subscriber threads alive. You can also replace it with a while loop as explained here.In ROS2 the concept is similar but it is termed executors and further extends it with so called callback groups.
This means that you actually have to register the subscribers and publisher with their callbacks inside a initialisation routine and then you keep your node alive for the desired time, either with a while loop or rospy.spin().
There is though no hard guarantee that all messages are actually processed. If your throughput is not sufficient (if your callbacks are executed too slowly) you will fill up the queue and eventually start dropping messages. If that is not what you need then have a look at the other communication methods - ROS services and actions. The other communication methods are though a bit more complicated.
Your code has several errors. You are lucky that Python is translated and not compiled and it never entered the relevant code parts..
subscriber, publisher and calculation are never called inside your code, meaning the publisher and subscriber are never registered.ros.Rate.sleep() and rospy.spin() have nothing to do inside the publisher anc calculation routines. They are used to put the thread into an endless loop as long as ROS is running. This happens once inside a program only and that should be the main loop for clarity!exact_pose has actually to use the local values/class members inside the class like self.line_pos and not the subscribers like self.line_pose_sub. The subscribers are only executed when they receive new data. You can't force a subscriber to check for new data the way you seem to try it!while and ros.spin() does not work in combination. In C++ there is ros::spinOnce() but there is no equivalent in Python. You therefore need to use while and rospy.Rate.sleep(d) instead!rospy.Rate.sleep requires a duration as first input argument and can't be left void.I tried to restructure it and the following code works:
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32
from math import cos,sin
class exact_pose():
def __init__(self):
# Set default values
self.car_yaw = 0.0
self.line_pose = 0.0
# Initialise node
rospy.init_node("emergency_procedure", anonymous=True)
r = rospy.Rate(1)
# Register topics
self.car_yaw_sub = rospy.Subscriber('car/yaw',Float32, self.callback_car_yaw)
self.line_pose_sub = rospy.Subscriber('line_pose',Float32, self.callback_line_pose)
self.exact_pose_pub = rospy.Publisher('exact_pose',Float32, queue_size=10)
# Repeat the following code in a loop
while not rospy.is_shutdown():
# Perform some calculation and publish to topic
self.exact_pose = self.line_pose * cos(self.car_yaw) + self.line_pose * sin(self.car_yaw)
info1 = Float32()
info1.data = self.exact_pose
self.exact_pose_pub.publish(info1)
# Sleep for some time
rospy.Rate.sleep(r)
return;
def callback_car_yaw(self, data):
# Copy data to class
self.car_yaw = data
return;
def callback_line_pose(self,data):
# Copy data to class
self.line_pose = data
return;
if __name__ == '__main__':
try:
# Create instance of class when node is called
e = exact_pose()
except rospy.ROSInterruptException:
pass
Put this code inside your code into your .py-file, open a new console and source the workspace with source devel/setup.bash, then start the roscore, open another console, source the workspace again and launch your Python node rosrun <package> <node>.py. Finally launch another console, source the workspace again and output the topic published by the Python node rostopic echo /exact_pose. If no other node is running this should output you 0.0 every second.