新增代码可以键盘遥控机械夹爪的上下摆动和夹爪开合

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -16,18 +16,22 @@ from ros_arduino_msgs.srv import ServoWrite
 
 msg = """
 从键盘读取控制命令遥控小车移动,控制按键如下:
-------------------
-   u    i    o
-   j    k    l
-   m    ,    .
-------------------
+-----------------------------
+     (左前) (前进) (右前)
+       u      i      o
+(左转) j      k      l (右转)
+       m      <      >
+     (左后) (后退) (右后)
+------------------------------
 q/z : 每次按照10%同比例来增加或减小线速度和角速度
 w/x : 每次按照10%比例来增加或减小线速度
 e/c : 每次按照10%比例来增加或减小角速度
 1/2 : 相机向上/向下转动
 3/4 : 相机向左/向右转动
-
-Ctrl+c: 退出键盘控制模式 
+5/6 : 夹爪向上/向下转动
+7/8 : 夹爪打开/关闭操作
+-------------------------------
+Ctrl+c: 退出键盘控制模式
 """
 
 moveBindings = {
@@ -60,6 +64,18 @@ cameraLeftBindings={
 cameraRightBindings={
         '4':(-5),
     }
+gripperUpBindings={
+        '5':(5),
+    }
+gripperDownBindings={
+        '6':(-5),
+    }
+gripperOpenBindings={
+        '7':(-5),
+    }
+gripperCloseBindings={
+        '8':(5),
+    }
 
 def getKey():
     tty.setraw(sys.stdin.fileno())
@@ -74,7 +90,7 @@ def vels(speed, turn):
 def angles(angle):
     return "当前角度:  %s " % (angle)
 
-#调用service: mobilebase_arduino/camera_control控制相机上下转动
+#调用service: mobilebase_arduino/servo_write控制对应舵机转动
 def client_srv(ser_id, ser_angle):
     rospy.wait_for_service("mobilebase_arduino/servo_write")
     try:
@@ -96,8 +112,10 @@ if __name__=="__main__":
     x = 0
     th = 0
     status = 0
-    upDownAngle = 90
-    leftRightAngle = 90
+    cameraUpDownAngle     = 90
+    cameraLeftRightAngle  = 90
+    gripperUpDownAngle    = 90
+    gripperOpenCloseAngle = 75
 
     try:
         print(msg)
@@ -117,31 +135,59 @@ if __name__=="__main__":
             elif key in cameraUpBindings.keys():
                 x = 0
                 th = 0
-                print(angles(upDownAngle))
-                if (upDownAngle <= 180 and upDownAngle > 0):
-                    upDownAngle = upDownAngle + cameraUpBindings[key]
-                    client_srv(1, upDownAngle)
+                print(angles(cameraUpDownAngle))
+                if (cameraUpDownAngle <= 180 and cameraUpDownAngle > 0):
+                    cameraUpDownAngle = cameraUpDownAngle + cameraUpBindings[key]
+                    client_srv(1, cameraUpDownAngle)
             elif key in cameraDownBindings.keys():
                 x = 0
                 th = 0
-                print(angles(upDownAngle))
-                if (upDownAngle < 180 and upDownAngle >= 0):
-                    upDownAngle = upDownAngle + cameraDownBindings[key]
-                    client_srv(1, upDownAngle)
+                print(angles(cameraUpDownAngle))
+                if (cameraUpDownAngle < 180 and cameraUpDownAngle >= 0):
+                    cameraUpDownAngle = cameraUpDownAngle + cameraDownBindings[key]
+                    client_srv(1, cameraUpDownAngle)
             elif key in cameraLeftBindings.keys():
                 x = 0
                 th = 0
-                print(angles(leftRightAngle))
-                if (leftRightAngle < 180 and leftRightAngle >=0):
-                    leftRightAngle = leftRightAngle + cameraLeftBindings[key]
-                    client_srv(0, leftRightAngle)
+                print(angles(cameraLeftRightAngle))
+                if (cameraLeftRightAngle < 180 and cameraLeftRightAngle >=0):
+                    cameraLeftRightAngle = cameraLeftRightAngle + cameraLeftBindings[key]
+                    client_srv(0, cameraLeftRightAngle)
             elif key in cameraRightBindings.keys():
                 x = 0
                 th = 0
-                print(angles(leftRightAngle))
-                if (leftRightAngle <= 180 and leftRightAngle >0):
-                    leftRightAngle = leftRightAngle + cameraRightBindings[key]
-                    client_srv(0, leftRightAngle)
+                print(angles(cameraLeftRightAngle))
+                if (cameraLeftRightAngle <= 180 and cameraLeftRightAngle >0):
+                    cameraLeftRightAngle = cameraLeftRightAngle + cameraRightBindings[key]
+                    client_srv(0, cameraLeftRightAngle)
+            elif key in gripperUpBindings.keys():
+                x = 0
+                th = 0
+                print(angles(gripperUpDownAngle))
+                if (gripperUpDownAngle < 130 and gripperUpDownAngle >= 65):
+                    gripperUpDownAngle = gripperUpDownAngle + gripperUpBindings[key]
+                    client_srv(2, gripperUpDownAngle)
+            elif key in gripperDownBindings.keys():
+                x = 0
+                th = 0
+                print(angles(gripperUpDownAngle))
+                if (gripperUpDownAngle <= 130 and gripperUpDownAngle > 65):
+                    gripperUpDownAngle = gripperUpDownAngle + gripperDownBindings[key]
+                    client_srv(2, gripperUpDownAngle)
+            elif key in gripperOpenBindings.keys():
+                x = 0
+                th = 0
+                print(angles(gripperOpenCloseAngle))
+                if (gripperOpenCloseAngle <= 120 and gripperOpenCloseAngle >75):
+                    gripperOpenCloseAngle = gripperOpenCloseAngle + gripperOpenBindings[key]
+                    client_srv(3, gripperOpenCloseAngle)
+            elif key in gripperCloseBindings.keys():
+                x = 0
+                th = 0
+                print(angles(gripperOpenCloseAngle))
+                if (gripperOpenCloseAngle < 120 and gripperOpenCloseAngle >=75):
+                    gripperOpenCloseAngle = gripperOpenCloseAngle + gripperCloseBindings[key]
+                    client_srv(3, gripperOpenCloseAngle)
             else:
                 x = 0
                 th = 0