#!/usr/bin/env python

from tactile_msgs.msg import TactileState
from sensor_msgs.msg import ChannelFloat32
from rosgraph_msgs.msg import Clock
from time import sleep
import rospy
import numpy

rospy.init_node('send_tactile_states')
pub = rospy.Publisher('tactile_states', TactileState, queue_size=1)

sim_time = rospy.get_param('use_sim_time', False)
clock_pub = rospy.Publisher('clock', Clock, queue_size=1)
clock_msg = Clock()

rate = rospy.Rate(10)
t = 0
period = 5

msg = TactileState()
msg.sensors.append(ChannelFloat32())
msg.sensors[0].name = rospy.get_param('~channel', 'tactile')
values = numpy.linspace(0, numpy.pi, num=rospy.get_param('~taxels', 64))
while not rospy.is_shutdown():
    stamp = rospy.Time.from_sec(t) if sim_time else rospy.Time.now()
    clock_msg.clock = stamp
    clock_pub.publish(clock_msg)
    print(stamp.secs)

    msg.header.stamp = stamp
    msg.sensors[0].values = 0.6 + 0.4 * numpy.sin(values + 2.*numpy.pi/period*t)
    pub.publish(msg)

    t += rate.sleep_dur.to_sec()
    sleep(rate.sleep_dur.to_sec())