修改机械夹爪控制，现为键盘控制12上升下降一定距离，45夹爪张开闭合一定角度

src/CMakeLists.txt

@@ -1 +1 @@
-/opt/ros/kinetic/catkin/cmake/toplevel.cmake
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -23,6 +23,7 @@ add_service_files(FILES
                   AlarmWrite.srv
                   LightShow.srv
                   GripperControl.srv
+                  GripperAngle.srv
                  )
 
 generate_messages(   

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperAngle.srv

@@ -0,0 +1,2 @@
+float32 angle
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperControl.srv

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

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -66,6 +66,9 @@ class ArduinoROS():
         # A service to position PWM servo
         rospy.Service('~gripper_control', GripperControl, self.GripperControlHandler)
 
+        # A service to control servo angle
+        rospy.Service('~gripper_angle', GripperAngle, self.GripperAngleHandler)
+
         # Initialize the controlller
         self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
@@ -150,9 +153,13 @@ class ArduinoROS():
         return LightShowResponse()
 
     def GripperControlHandler(self, req):
-        self.controller.gripper_control(req.value, req.angle)
+        self.controller.gripper_control(req.value)
         return GripperControlResponse()
 
+    def GripperAngleHandler(self, req):
+        self.controller.gripper_angle(req.angle)
+        return GripperAngleResponse()
+
     # Stop the robot
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")

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

@@ -344,11 +344,17 @@ class Arduino:
         except:
             return None
 
-    def gripper_control(self, operation, angle):
+    def gripper_control(self, operation):
         ''' 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 %d' %(operation, angle))
+        return self.execute_ack('z %d' %(operation))
+
+    def gripper_angle(self, angle):
+        ''' control gripper angle
+        '''
+        #rospy.loginfo("gripper operation ID:" + str(operation))
+        return self.execute_ack('s 1 %d' %(angle))
 
     def drive(self, AWheel, BWheel, CWheel):
         ''' Speeds are given in encoder ticks per PID interval

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -5,6 +5,7 @@
 # Description: 通过键盘遥控小车行进、抓取。
 # History:
 #   20200317 增加小车抓取功能 by jally
+#   20200408 增加小车可通过“7”“8”数字键控制夹爪张开闭合角度
 
 from __future__ import print_function
 
@@ -15,6 +16,8 @@ from geometry_msgs.msg import Twist
 import sys, select, termios, tty
 
 from ros_arduino_msgs.srv import GripperControl
+from ros_arduino_msgs.srv import GripperAngle
+import time
 
 msg = """
 Reading from the keyboard  and Publishing to Twist!
@@ -38,6 +41,7 @@ anything else : stop
 q/z : increase/decrease max speeds by 10%
 w/x : increase/decrease only linear speed by 10%
 e/c : increase/decrease only angular speed by 10%
+7/8 : increase/decrease angle by 5°
 
 1 : open the gripper and down
 2 : stop
@@ -78,12 +82,26 @@ speedBindings={
         'c':(1,.9),
     }
 
-gripperBindings={
+
+#gripperBindings={
+#        '1':(),
+#        '2':(),
+#        '3':(),
+#        '4':(),
+#        '5':(),
+#    }
+
+gripperupBindings={
         '1':(),
+    }
+gripperdownBindings={
         '2':(),
-        '3':(),
-        '4':(),
-        '5':(),
+    }
+gripperopenBindings={
+        '4':(5),
+    }
+grippercloseBindings={
+        '5':(-5),
     }
 
 def getKey():
@@ -97,12 +115,20 @@ def getKey():
 def vels(speed,turn):
     return "currently:\tspeed %s\tturn %s " % (speed,turn)
 
+def angles(angle):
+    return "currently:\tangle %s " % (angle)
+
+
 #调用service: mobilebase_arduino/gripper_control
 def client_srv():
     rospy.wait_for_service("mobilebase_arduino/gripper_control")
+    rospy.wait_for_service("mobilebase_arduino/gripper_angle")
     try:
         gripper_client = rospy.ServiceProxy("mobilebase_arduino/gripper_control",GripperControl)
-        resp = gripper_client.call(int(key))
+        gripper2_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle",GripperAngle)
+#        resp = gripper_client.call(int(key))
+        resp = gripper_client.call(command)
+        resp = gripper2_client.call(angle)
     except rospy.ServiceException, e:
         rospy.logwarn("Service call failed: %s"%e)
 
@@ -121,6 +147,8 @@ if __name__=="__main__":
     z = 0
     th = 0
     status = 0
+    angle = 10
+    command = 2
 
     try:
         print(msg)
@@ -139,13 +167,56 @@ if __name__=="__main__":
                 print(vels(speed,turn))
                 if (status == 14):
                     print(msg)
-                status = (status + 1) % 15
-            elif key in gripperBindings.keys():
+                status = (status + 1) % 15           
+#            elif key in gripperBindings.keys():
+#                x = 0
+#                y = 0
+#                z = 0
+#                th = 0
+#                client_srv()
+            elif key in gripperupBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
+                command = 3
+                print("currently:\tgo up")
                 client_srv()
+                time.sleep(0.2)
+                command = 2
+                print("currently:\tstop !")
+                client_srv()          
+            elif key in gripperdownBindings.keys():
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                command = 1
+                print("currently:\tgo down")
+                client_srv()
+                time.sleep(0.2)
+                command = 2
+                print("currently:\tstop !")
+                client_srv()
+            elif key in gripperopenBindings.keys():
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                print(angles(angle))
+                if (angle < 65 and angle >=0):
+                    angle = angle + gripperopenBindings[key]
+                    client_srv()
+            elif key in grippercloseBindings.keys():
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                print(angles(angle))
+                if (angle <= 65 and angle >0):
+                    angle = angle + grippercloseBindings[key]
+                    client_srv()
+             
             else:
                 x = 0
                 y = 0