Nextage and uArm demo

#!/usr/bin/env python

import time
import rospy
from hrpsys_ros_bridge.srv import *
import math
from uarm.msg import Joints
from std_msgs.msg import Bool


class MoveNextage:

    def __init__(self):
        rospy.loginfo('start wait')
        rospy.wait_for_service('/SequencePlayerServiceROSBridge/setTargetPose')
        self._set_target_pose = rospy.ServiceProxy(
            '/SequencePlayerServiceROSBridge/setTargetPose',
            OpenHRP_SequencePlayerService_setTargetPose)
        rospy.wait_for_service(
            '/SequencePlayerServiceROSBridge/waitInterpolation')
        self._wait_interpolation = rospy.ServiceProxy(
            '/SequencePlayerServiceROSBridge/waitInterpolation',
            OpenHRP_SequencePlayerService_waitInterpolation)
        rospy.loginfo('end wait')

    def move_to(self, arm, position, time):
        result = self._set_target_pose(
            arm, position, [3.0, -math.pi / 2, -3.0], time)
        print result
        self._wait_interpolation()

    def pick(self):
        self.move_to('rarm', [0.4, -0.3, 1.3], 3.0)
        self.move_to('rarm', [0.4, -0.3, 1.0], 3.0)
        self.hold(True)
        time.sleep(3.0)
        self.move_to('rarm', [0.4, -0.3, 1.3], 1.5)
        self.move_to('rarm', [0.4, -0.1, 1.3], 1.5)
        self.move_to('rarm', [0.4, -0.1, 1.0], 1.5)
        self.hold(False)
        time.sleep(3.0)
        self.move_to('rarm', [0.4, -0.1, 1.3], 1.5)

    def hold(self, is_hold):
        # TODO
        pass


class MoveUarm(object):

    def __init__(self):
        self._pub = rospy.Publisher('/uarm/joint_commands', Joints)
        self._gripper_pub = rospy.Publisher('/uarm/gripper', Bool)

    def move(self, a, b, c, d):
        msg = Joints(a, b, c, d)
        self._pub.publish(msg)
        time.sleep(1.0)

    def pick(self):
        self.move(30.0, 30.0, -45.0, 0.0)
        self.move(180, 30, -45, 0)
        self.hold(True)
        self.move(180, -50, -45, 0)
        self.move(180, 50, -45, 0)
        self.move(180, 50, 45, 0)
        self.hold(False)
        self.move(30, 30, 0, 0)

    def hold(self, is_hold):
        msg = Bool(is_hold)
        self._gripper_pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node('move_nextage_otl')
    uarm = MoveUarm()
    uarm.pick()
    nextage = MoveNextage()
    nextage.pick()
    rospy.spin()