#!/usr/bin/env python
#coding=utf-8
#女生节礼物
 
import rospy
from sensor_msgs.msg import LaserScan
import numpy
import copy
 
node_name = "Test_Maker"
 
class Test_Maker():
  def __init__(self):
    self.Define()
    rospy.Timer(rospy.Duration(0.5), self.Timer_CB1)
    rospy.Timer(rospy.Duration(0.5), self.Timer_CB2)
    rospy.Timer(rospy.Duration(0.5), self.Timer_CB3)
    rospy.Timer(rospy.Duration(0.5), self.Timer_CB4)
    rospy.spin()
 
  def Define(self):
    self.pub_scan1 = rospy.Publisher('test/test_scan1', LaserScan, queue_size=1)
    self.pub_scan2 = rospy.Publisher('test/test_scan2', LaserScan, queue_size=1)
    self.pub_scan3 = rospy.Publisher('test/test_scan3', LaserScan, queue_size=1)
    #慎用！！！！
    self.pub_scan4 = rospy.Publisher('test/test_scan4', LaserScan, queue_size=1)
 
  def Timer_CB1(self, e):
    data = LaserScan()
    data.header.frame_id = "base_link"
    data.angle_min = 0
    data.angle_max = numpy.pi*2
    data.angle_increment = numpy.pi*2 / 200
    data.range_max = numpy.Inf
    data.range_min = 0
    theta = 0
    for i in range(200):
      r = 8.* numpy.sin(5. * theta )
      data.ranges.append(copy.deepcopy(r))
      data.intensities.append(theta)
      r = 8.* numpy.sin(5. * -theta)
      data.ranges.append(copy.deepcopy(r))
      data.intensities.append(theta)
 
      theta += data.angle_increment
    data.header.stamp = rospy.Time.now()
    self.pub_scan1.publish(data)
 
  def Timer_CB2(self, e):
    data = LaserScan()
    data.header.frame_id = "base_link"
    data.angle_min = 0
    data.angle_max = numpy.pi*2
    data.angle_increment = numpy.pi*2 / 200
    data.range_max = numpy.Inf
    data.range_min = 0
    theta = 0
    for i in range(200):
      r = 8. * numpy.cos(5. * theta) + 1
      data.intensities.append(theta)
      data.ranges.append(copy.deepcopy(r))
      r = 8. * numpy.cos(5. * -theta) + 1
      data.intensities.append(theta)
      data.ranges.append(copy.deepcopy(r))
      theta += data.angle_increment
 
    data.header.stamp = rospy.Time.now()
    self.pub_scan2.publish(data)
 
  def Timer_CB3(self, e):
    data = LaserScan()
    data.header.frame_id = "base_link"
    data.angle_min = 0
    data.angle_max = numpy.pi*2
    data.angle_increment = numpy.pi*2 / 50
    data.range_max = numpy.Inf
    data.range_min = 0
    theta = 0
    for i in range(200):
      r = 2. * numpy.sin(5. * theta) + 1
      data.intensities.append(theta)
      data.ranges.append(copy.deepcopy(r))
      r = 2. * numpy.sin(5. * -theta) + 1
      data.intensities.append(theta)
      data.ranges.append(copy.deepcopy(r))
      theta += data.angle_increment
 
    data.header.stamp = rospy.Time.now()
    self.pub_scan3.publish(data)
 
  #慎用！！！！
  def Timer_CB4(self, e):
    data = LaserScan()
    data.header.frame_id = "base_link"
    data.angle_min = 0
    data.angle_max = numpy.pi*2
    data.angle_increment = data.angle_max / 200
    data.range_max = numpy.Inf
    data.range_min = 0
    theta = 0
    for i in range(200):
      r = 9. * numpy.arccos(numpy.sin(theta)) + 9
      data.ranges.append(r)
      theta += data.angle_increment
 
    data.header.stamp = rospy.Time.now()
    self.pub_scan4.publish(data)
 
if __name__ == '__main__':
  node_name = 'Test_Maker'
  rospy.init_node(node_name)
  try:
    Test_Maker()
  except rospy.ROSInterruptException:
    rospy.logerr('%s error'%node_name)