[MG400, Python] ROSでMG400を動かす方法

#!/usr/bin/env python

import rospy
from mg400_bringup.srv import *
from mg400_bringup.msg import RobotStatus

class Move:
    def __init__(self):
        # 新しいノードを作る
        rospy.init_node('MG400_work')
        # MG400 のステータスを格納する変数
        self.status = 0
        # 使用するサービスを呼ぶハンドラー
        self.enable = rospy.ServiceProxy('mg400_bringup/srv/EnableRobot', EnableRobot)
        self.moveJ = rospy.ServiceProxy('mg400_bringup/srv/MovJ', MovJ)
        self.disable = rospy.ServiceProxy('mg400_bringup/srv/DisableRobot', DisableRobot)
        self.clear_error = rospy.ServiceProxy('mg400_bringup/srv/ClearError', ClearError)
        # RobotStatusトピックを購読
        rospy.Subscriber("mg400_bringup/msg/RobotStatus", RobotStatus, self.robotStatus_callback)
        self.rate = rospy.Rate(20)

    # 指定の時間スリープさせる
    def sleep(self, duration):
        now = rospy.Time().now()
        while now + rospy.Duration(duration) > rospy.Time().now():
            self.rate.sleep()


    # エラーが発生した際に、リスタートをするか選択させる
    def restart(self):
        answer = input("Error detected. Do you want to continue? [y/N]: ").lower()
        self.sleep(2)
        self.disable()
        self.clear_error()
        if answer in ['y', 'yes']:
            self.enable()
            self.sleep(2)
        elif answer in ['n', 'no']:
            self.sleep(2)
            self.disable()
            sys.exit(1)  # プログラムを終了される


    # 購読したトピックのメッセージで変数をアップデートする
    def robotStatus_callback(self, robot_status):
        """
            1:  "ROBOT_MODE_INIT",
            2:  "ROBOT_MODE_BRAKE_OPEN",
            3:  "",
            4:  "ROBOT_MODE_DISABLED",
            5:  "ROBOT_MODE_ENABLE",
            6:  "ROBOT_MODE_BACKDRIVE",
            7:  "ROBOT_MODE_RUNNING",
            8:  "ROBOT_MODE_RECORDING",
            9:  "ROBOT_MODE_ERROR",
            10: "ROBOT_MODE_PAUSE",
            11: "ROBOT_MODE_JOG"
        """
        self.status = robot_status.robot_status
       

    # 前の動きが止まるまで待つ
    def wait(self):
        if self.status == 9:  # エラーのときは restart()を呼ぶ
            self.restart()
        self.sleep(0.5)
        while self.status !=5:
            if self.status == 9:
                self.restart()
                break
            self.rate.sleep()


    # ロボットを動かす
    def move(self):
        # 指定のサービスが使えるようになるまで待機
        rospy.wait_for_service('mg400_bringup/srv/EnableRobot')  
        try:
            self.enable()
            self.wait()
            self.sleep(1)
            for _ in range(10):  # 10サイクル繰り返す
                self.moveJ(280, 165, -60, 30)
                self.wait()
                self.moveJ(275, -170, -30, -30)
                self.wait()
                self.moveJ(220, -35, 48, -10)
                self.wait()      
            self.disable()        
   
        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)


if __name__ == "__main__":  
    print("MG400 sample start")
    mv = Move()
    mv.move()
    rospy.spin()