Subscriber in /odom topic from gazebo loses first message

Question

Hello!

I have a robot in a gazebo environment that publishes the odometry data on the topic /odom. I have a node that subscribes to that topic and uses the odometry data. I save the data from the topic in variables, right after the first message arrives. I found out that the first received message does't include the odometry data (it gets nothing) and my variables remain at the initial values i gave. From the 2nd iteration and on the messages arrive with the data without any problem. From what I searched so far, I found out that it takes some time for the subscriber to make the connection to the topic and this results to missing the first message. In most of the applications the first message is omitted. But if it is necessary to get the first message I can use the latch=True in the publisher creation, which saves somehow the last published message every time a new subscriber connects to that topic.

#!/usr/bin/env python
import rospy
.
.
.

rospy.init_node('face_finder', anonymous=True)

marker_publisher = rospy.Publisher('box_visual', Marker, queue_size=100)
rospy.sleep(1)

odom = Odometry_data(0.0, 0.0, 0.0)

def get_odom(msg):
    print "inside the get_odom", '\n'
    # print msg.pose.pose
    x_odom = msg.pose.pose.position.x
    y_odom = msg.pose.pose.position.y
    quat_x = msg.pose.pose.orientation.x
    quat_y = msg.pose.pose.orientation.y
    quat_z = msg.pose.pose.orientation.z
    quat_w = msg.pose.pose.orientation.w
    euler = tf.transformations.euler_from_quaternion([quat_x, quat_y, quat_z, quat_w])
    # print "euler = ", euler
    theta_odom = euler[2]
    # print theta_odom

    # Transform the odometry data wrt the hokuyo_link frame
    x_odom_hokuyo = x_odom + 0.377
    global odom
    odom = Odometry_data(x_odom_hokuyo, y_odom, -theta_odom)
    print "odom_2 = ", odom.x, ' ', odom.y, ' ', odom.theta, '\n'

def callback(data):
    .
    .
    .

def face_finder():

    rospy.Subscriber("/odom", Odometry, get_odom)
    print "odom_2 = ", odom.x, ' ', odom.y, ' ', odom.theta, '\n'
    rospy.Subscriber("/line_segments", LineSegmentList, callback)
    rospy.spin()

if __name__ == '__main__':
    face_finder()

I don't mind if I miss the first message, but I want to have correct data when the callback is called (this is where I use the odometry data). Also, I cannot put any rospy.sleep() after the subscriber call because it is inside my main loop and it will slow down the system. If I put the subscriber call before the main loop then I will get the message only once at the beginning (right?). And finally, if latch=True is the solution, how can I have access to the gazebo node that publishes the message and change the argument?

What approach would be more suitable for my case? Any hint or advice is appreciated.

I use Ubuntu 16.04 LTS, ROS-kinetic, and python 2.7.

Thank you in advance!!!

---

Originally posted by Spyros on ROS Answers with karma: 51 on 2020-10-15

Post score: 0

Answer 1

I don't know if there is a more clever way to solve this (i.e. add an argument in the subscriber call to save previous messages, or change the way and the order I call the subscriber). I managed to solve this by adding a simple "if" statement and a "return" at the beginning of my callback function. If the odometry data are still equal to the initial values I gave then stop this iteration and try again in the next one.

---

Originally posted by Spyros with karma: 51 on 2020-10-22

This answer was ACCEPTED on the original site

Post score: 0