reem_head_control_joystick.py

#!/usr/bin/env python
# -*- coding:utf-8 -*-


import rospy
from sensor_msgs.msg import Joy
from control_msgs.msg import JointTrajectoryControllerState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

JOY_TOPIC = "/joy"
HEAD_STATE_TOPIC = "/head_controller/state"
HEAD_COMMAND_TOPIC = "/head_controller/command"

MOVE_AMOUNT = 0.1

class ReemHeadTeleop(object):
    def __init__(self):
        self.last_joy_msg = None
        self.new_joy = False
        self.last_state = None
        self.joy_sub = rospy.Subscriber(JOY_TOPIC, Joy, self.joy_cb, queue_size=1)
        self.head_state_sub = rospy.Subscriber(HEAD_STATE_TOPIC, JointTrajectoryControllerState,
                                               self.state_cb, queue_size=1)
        self.head_command_pub = rospy.Publisher(HEAD_COMMAND_TOPIC, JointTrajectory, queue_size=1)

    def joy_cb(self, data):
        self.last_joy_msg = data
        self.new_joy = True


    def state_cb(self, data):
        self.last_state = data

    def move_head(self):
# joint_names: ['head_2_joint', 'head_1_joint']
# points:
#   -
#     positions: [0.0, -0.4]
#     velocities: [0.0, 0.0]
#     accelerations: []
#     effort: []
#     time_from_start:
#       secs: 2
#       nsecs: 0

        if self.new_joy == False:
            return
        else:
            self.new_joy = False

        if self.last_joy_msg.axes[4] == 0.0:
            return

        jt = JointTrajectory()
        jt.joint_names = ['head_2_joint', 'head_1_joint']
        jtp = JointTrajectoryPoint()
        jtp.positions.append(0.0)
        objective_pose = self.last_state.actual.positions[0] # head joint 1
        direction = self.last_joy_msg.axes[4]
        objective_pose += MOVE_AMOUNT * direction
        jtp.time_from_start = rospy.Duration(0.25)

        jtp.positions.append(objective_pose)
        jtp.velocities.append(0.0)
        jtp.velocities.append(0.0)
        jt.points.append(jtp)
        rospy.loginfo("Sending: " + str(jtp))
        self.head_command_pub.publish(jt)



    def run(self):
        rospy.loginfo("Waiting for first joy and state messages")
        while self.last_joy_msg is None or self.last_state is None:
            rospy.sleep(0.5)

        rospy.loginfo("Done, controlling")
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.move_head()
            r.sleep()



if __name__ == "__main__":
    rospy.init_node('joy_head_teleop_reem')
    rht = ReemHeadTeleop()
    rht.run()