kobuki /odom can't receive at the first time
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()
Asked by kai1006 on 2016-03-04 01:35:42 UTC
Comments