#!/usr/bin/env python
# license removed for brevity
import rospy, time
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def callback(data):
	rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)

def listener():

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("chatter", String, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
	# names this node created by script	
	rospy.init_node('talker', anonymous=True)       	 
	
	# MOVING THE TORSO	
	# topic I'm publising to
	pub = rospy.Publisher('/torso_controller/command', JointTrajectory, queue_size=10) 

    	torso_message = JointTrajectory()
        torso_message.joint_names = ['torso_lift_joint']
	torso_point = JointTrajectoryPoint()
	
	# raise up slightly
        torso_point.positions = [2] 
	torso_point.time_from_start = rospy.Duration(1)
	torso_message.points.append(torso_point)

	time.sleep(2)
	pub.publish(torso_message)


