#!/usr/bin/env python

import rospy
from sensor_msgs.msg import JointState

from markers import *
from functions import *

rospy.init_node("testForwardKinematics")
pub = rospy.Publisher('joint_states', JointState, queue_size=1000)
bmarker = BallMarker(color['GREEN'])

# Joint names
jnames = ['joint1', 'joint2', 'joint3','joint4', 'joint5', 'joint6']
# Joint Configuration

q = np.array([-1.15, 1.03, -2.5, 3.5, -3.31, 2.4])
#q = np.array([1.5, 0.73, -1.5, 2.5, -2.31, 0.4])
#q = [0, 0, 0, 0, 0, 0]
# -1.75 < q[0] < 2.79
# -0.52 < q[1] < 1.22
# -2.97 < q[2] < 1.05
# -6.11 < q[3] < 6.11
# -3.58 < q[4] < 0.63
# -6.28 < q[5] < 6.28
# End effector with respect to the base
T = fkine(q)
print( np.round(T, 3) )
bmarker.position(T)

# Object (message) whose type is JointState
jstate = JointState()
# Set values to the message
jstate.header.stamp = rospy.Time.now()
jstate.name = jnames
# Add the head joint value (with value 0) to the joints
jstate.position = q

# Loop rate (in Hz)
rate = rospy.Rate(100)
# Continuous execution loop
while not rospy.is_shutdown():
    # Current time (needed for ROS)
    jstate.header.stamp = rospy.Time.now()
    # Publish the message
    pub.publish(jstate)
    bmarker.publish()
    # Wait for the next iteration
    rate.sleep()