00001
00002
00003 import roslib; roslib.load_manifest('rviz')
00004 from nav_msgs.msg import Path
00005 from geometry_msgs.msg import PoseStamped
00006 import math
00007 import rospy
00008
00009 topic = 'test_path'
00010 publisher = rospy.Publisher(topic, Path)
00011
00012 rospy.init_node('send_path')
00013
00014 t = 0
00015 while not rospy.is_shutdown():
00016
00017 p = Path()
00018 p.header.frame_id = "/base_link"
00019 p.header.stamp = rospy.Time.now()
00020
00021 num_points = 100
00022 for i in range( 0, num_points ):
00023 ps = PoseStamped()
00024 ps.header.stamp = p.header.stamp
00025 ps.header.frame_id = p.header.frame_id
00026 ps.pose.position.x = 10.0 * i / num_points - 5
00027 ps.pose.position.y = math.sin( 10.0 * i / num_points + t )
00028 ps.pose.position.z = 0
00029 ps.pose.orientation.x = 0
00030 ps.pose.orientation.y = 0
00031 ps.pose.orientation.z = 0
00032 ps.pose.orientation.w = 1
00033 p.poses.append( ps )
00034
00035 publisher.publish( p )
00036
00037 t += .1
00038
00039 rospy.sleep(0.03)