#! /usr/bin/env python
import rospy
import sys
from std_msgs.msg import Float32

class controlArm:	
    def __init__(self):
        self.ref=0

    def update(self):
        pub=rospy.Publisher('climber/arm_state',Float32, queue_size=1000)
        sub=rospy.Subscriber('climber/arm_state',Float32,self.pose_callback,queue_size=1)
        msg=Float32()

        c=sys.stdin.read(1)
        if c=='z':
            msg.data=self.ref-0.05

        elif c=='x':
            msg.data=self.ref+0.05

        else :
            msg.data=self.ref   
        
        pub.publish(msg)

        
    
    def pose_callback(self, msg):
        self.ref=msg.data
    
		
if __name__== "__main__":	
	print ("Keyboard Control Node:")
	cnt=controlArm()

	rospy.init_node('Arm_Controller', anonymous=False)
	while not rospy.is_shutdown():
		cnt.update()
		rospy.Rate(100).sleep()
