run-time update of odometry message while publishing to a new topic
hello everyone. I am trying to publish odometry messages over a new topic. I am having a small problem. I am subscribing to odom topic
, thhen creating a new topic and publishing the message with some change. The code is working. But when i run rostopic echo odom2
, it just gives me constant values and not the updated changing values. I am subscribing the newly published topic also, but data is not being updated in run time. Below is my code. What could be the posssible reason and how can i resolve the issue.
#!/usr/bin/env python
import rospy
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry
def listener():
rospy.init_node('move_turtlebot', anonymous=True)
rospy.Subscriber("odom", Odometry, move_turtlebot)
rospy.spin()
def move_turtlebot(data):
pub = rospy.Publisher('odom2', Odometry, queue_size=10)
move_turtlebot= Odometry()
data.header.stamp= rospy.Time.now()
data.header.frame_id = "odom"
data.pose.covariance= [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pub.publish(data)
rospy.sleep(0.25)
if __name__ == '__main__':
try:
listener()
except rospy.ROSInterruptException:
pass
Asked by enthusiast.australia on 2019-10-23 16:58:20 UTC
Answers
One thing I would definitely recommend changing is to not create the publisher inside the subscriber callback. Creating a new publisher each time is going to result in lost messages (as it takes a certain amount of time for subscribers to connect to that new publisher).
Second bug (that I found as I modified your code) is that you re-assign "move_turtlebot" to an odometry message - so you only end up processing one message before you probably are breaking your function by reassigning it to a message instance.
I'd recommend anytime you need to access variables from outside the callback, you should use a class (as opposed to a bunch of "global" usage).
This code runs, but is untested against actual messages:
#!/usr/bin/env python
import rospy
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry
class OdometryModifier:
def __init__(self):
self.sub = rospy.Subscriber("odom", Odometry, self.callback)
self.pub = rospy.Publisher('odom2', Odometry, queue_size=10)
def callback(self, data):
data.header.stamp= rospy.Time.now()
data.header.frame_id = "odom"
data.pose.covariance= [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.pub.publish(data)
if __name__ == '__main__':
try:
rospy.init_node('move_turtlebot', anonymous=True)
odom = OdometryModifier()
rospy.spin()
except rospy.ROSInterruptException:
pass
Asked by fergs on 2019-10-23 21:45:42 UTC
Comments
One thing I would definitely recommend changing is to not create the publisher inside the subscriber callback.
+100 to this.
Creating Publisher
s and Subscriber
s in callbacks has to be the nr 1 issue with code here on ROS Answers. It's apparently not apparent that this does not work (reliably).
Asked by gvdhoorn on 2019-10-24 03:23:58 UTC
So how should i modify things? I am not very experienced , so i will appreciate your help. Also in the tutorial of publishing and subscribing, it is using callback in subscriber. SO, what changes do i need to make to get the things going?
Asked by enthusiast.australia on 2019-10-24 05:01:58 UTC
Also in the tutorial of publishing and subscribing, it is using callback in subscriber.
yes, it registers a callback with the Subcriber
.
it does not create the rospy.Publisher
inside the callback.
That is what @fergs is referring to.
Asked by gvdhoorn on 2019-10-24 07:49:43 UTC
So, any suggestions what should i do?I believe from the code it is clear what i am trying to do. So what should i do to make it running?
Asked by enthusiast.australia on 2019-10-24 08:51:32 UTC
I updated the answer with a bit of (untested other than running it at the command line to make sure I had no syntax errors) code, but it should move you in the right direction
Asked by fergs on 2019-10-24 20:07:11 UTC
Comments