增加夹爪合拢上行时可输入合拢角度

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperControl.srv

@@ -1,2 +1,3 @@
 float32 value
+float32 angle
 ---

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -150,7 +150,7 @@ class ArduinoROS():
         return LightShowResponse()
 
     def GripperControlHandler(self, req):
-        self.controller.gripper_control(req.value)
+        self.controller.gripper_control(req.value, req.angle)
         return GripperControlResponse()
 
     # Stop the robot

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -344,11 +344,11 @@ class Arduino:
         except:
             return None
 
-    def gripper_control(self, operation):
+    def gripper_control(self, operation, angle):
         ''' control gripper: 1-open gripper down 2:stop 3:close gripper up 4:open gripper 5:close gripper
         '''
         #rospy.loginfo("gripper operation ID:" + str(operation))
-        return self.execute_ack('z %d' %(operation))
+        return self.execute_ack('z %d %d' %(operation, angle))
 
     def drive(self, AWheel, BWheel, CWheel):
         ''' Speeds are given in encoder ticks per PID interval