add jz

jz_dev/jz_dev/jz_dev_node.py

@@ -14,7 +14,6 @@ import sys
 import os
 
 class JzDevNode(Node):
-    def __init__(self):
     def __init__(self):
         super().__init__('jz_dev_node')
         self.declare_parameter('port','/dev/ttyUSB0')
@@ -54,6 +53,11 @@ class JzDevNode(Node):
             ret = self.clawControl.enableClamp(self.devid, True)
             if ret!=1:
                 print('enble fail!')
+                result=JzCmd.Result()
+                result.state="jz enable fail!"
+                result.result=0
+                goal.abort()
+                return result
             print('start run...')
             self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
             ##feedback