#!/usr/bin/env python

import roslib; roslib.load_manifest('rviz')
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import math
import rospy
import tf.transformations as tf

topic = 'test_path'
publisher = rospy.Publisher(topic, Path, queue_size=1)

rospy.init_node('send_path')

t = 0
rate = rospy.Rate(30)
timestep = rate.sleep_dur.to_sec()
while not rospy.is_shutdown():

   p = Path()
   p.header.frame_id = "/base_link"
   p.header.stamp = rospy.Time.now()
   
   num_points = 50
   for i in range( 0, num_points ):
      ps = PoseStamped()
      ps.header.stamp = p.header.stamp
      ps.header.frame_id = p.header.frame_id
      ps.pose.position.x = 10.0 * i / (num_points-1) - 5
      ps.pose.position.y = math.sin( 10.0 * i / num_points + t )
      ps.pose.position.z = 0
      angle = 2 * math.pi * i / num_points + t
      quat = tf.quaternion_from_euler(angle, 0, 0)
      ps.pose.orientation.x = quat[0]
      ps.pose.orientation.y = quat[1]
      ps.pose.orientation.z = quat[2]
      ps.pose.orientation.w = quat[3]
      p.poses.append( ps )

   publisher.publish( p )

   t += timestep
   rate.sleep()
