close service, add logs

jz_dev/jz_dev/jz_dev_node.py

@@ -13,11 +13,11 @@ import os
 class JzDevNode(Node):
     def __init__(self):
         super().__init__('jz_dev_node')
-        self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
+        # self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
         self.action=ActionServer(self,JzCmd,'jz_cmd',self.jz_cmd_callback_action)
         self.declare_parameter('port','/dev/ttyUSB0')
         param = self.get_parameter('port')
-        #print('input param:',param.value)
+        print('input param:',param.value)
         self.com_dev = param.value
         self.clawControl = RgClawControl()
         self.target_loc=0
@@ -32,6 +32,7 @@ class JzDevNode(Node):
         print('jz_dev_node init:',self.com_dev)
         #self.feedback_timer = self.create_timer(0.1, self.update_feedback) 
     def jz_cmd_callback_action(self,goal):
+        print("jz cmd callback")
         self.cur_goal=goal
         req=goal.request
         self.cur_id=req.devid
@@ -75,6 +76,7 @@ class JzDevNode(Node):
             result.devid=self.cur_id
             result.state="uart open fail!"
             goal.abort()
+            print("uart open failed")
             return result
         #print('start timer')
         self.timer_on=True