Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('rviz')
00004 from nav_msgs.msg import Odometry
00005 import rospy
00006
00007 topic = 'test_odometry'
00008 publisher = rospy.Publisher(topic, Odometry)
00009
00010 rospy.init_node('send_odometry')
00011
00012 y = 0
00013 while not rospy.is_shutdown():
00014
00015 odo = Odometry()
00016 odo.header.frame_id = "/base_link"
00017 odo.header.stamp = rospy.Time.now()
00018
00019 odo.pose.pose.position.x = 0
00020 odo.pose.pose.position.y = y
00021 odo.pose.pose.position.z = 0
00022
00023 odo.pose.pose.orientation.x = 0
00024 odo.pose.pose.orientation.y = 0
00025 odo.pose.pose.orientation.z = 0
00026 odo.pose.pose.orientation.w = 1
00027
00028 publisher.publish( odo )
00029
00030 y = y + .2
00031 if y > 5:
00032 y = -5
00033
00034 rospy.sleep(0.1)