kobuki /odom can't receive at the first time

asked 2016-03-04 00:35:42 -0500

kai1006 gravatar image

Hi all,

I write a simple node to receive "/odom". After I launched the kobuki_nodes/minimal.launch, the callback function won't work. But if I restart my node, the callback function works. Or if I move the kobuki's wheels before I run my node, it works. How can I solve this problem?

Here is the code

sub_odom():
    sub = rospy.Subscriber('/odom',Odometry, callback_odom)
def callback_odom(data):
    global x,y,th
    x = data.pose.pose.position.x
    y = data.pose.pose.position.y
    q1 = data.pose.pose.orientation.x
    q2 = data.pose.pose.orientation.y
    q3 = data.pose.pose.orientation.z
    q4 = data.pose.pose.orientation.w
    q = (q1, q2, q3, q4)
    e = euler_from_quaternion(q)
    th = degrees(e[2])
    th = to_positive_angle(th)

    x, y, th = 0.0, 0.0, 0.0

rospy.init_node("sub_odom")
rate = rospy.Rate(5) 
## Reset odometry
pub = rospy.Publisher('/mobile_base/commands/reset_odometry' ,Empty, queue_size=10)
pub.publish()
time.sleep(1)
if __name__ == '__main__':
    sub_odom()
    while not rospy.is_shutdown():
        print "x:%.2f y:%.2f heading:%.2f"%(x, y, th)
        rate.sleep()
edit retag flag offensive close merge delete