更新键盘遥控代码,将舵机控制代码精简优化

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -2,14 +2,14 @@
 # coding:utf-8
 
 # Copyright: 2016-2021 wwww.corvin.cn ROS小课堂
-# Description: 通过键盘遥控小车行进，摄像头舵机转动。
+# Description: 通过键盘遥控小车行进，摄像头舵机转动,2DOF机械臂转动.
 # History:
 #    20200726:增加控制左右转动舵机的代码.
 #    20210913:优化代码.
+#    20211117:将控制舵机转动的代码进行整合优化,精简代码.
 
 from __future__ import print_function
-import roslib; roslib.load_manifest('teleop_twist_keyboard')
-import rospy
+import rospy, roslib; roslib.load_manifest('teleop_twist_keyboard')
 from geometry_msgs.msg import Twist
 import sys, select, termios, tty
 from ros_arduino_msgs.srv import ServoWrite
@@ -46,37 +46,30 @@ moveBindings = {
     }
 speedBindings={
         'q':(1.1,1.1),
-        'z':(.9,.9),
+        'z':(0.9,0.9),
         'w':(1.1,1),
-        'x':(.9,1),
+        'x':(0.9,1),
         'e':(1,1.1),
-        'c':(1,.9),
+        'c':(1,0.9),
     }
-cameraUpBindings={
+cameraUpDownBindings={
         '1':(-5),
-    }
-cameraDownBindings={
         '2':(5),
     }
-cameraLeftBindings={
+cameraLeftRightBindings={
         '3':(5),
-    }
-cameraRightBindings={
         '4':(-5),
     }
-gripperUpBindings={
+gripperUpDownBindings={
         '5':(5),
-    }
-gripperDownBindings={
         '6':(-5),
     }
-gripperOpenBindings={
+gripperOpenCloseBindings={
         '7':(-5),
-    }
-gripperCloseBindings={
         '8':(5),
     }
 
+#等待键盘按键被按下,获取到按键后将其返回
 def getKey():
     tty.setraw(sys.stdin.fileno())
     select.select([sys.stdin], [], [], 0)
@@ -109,19 +102,26 @@ if __name__=="__main__":
     speed = 0.2
     turn = 1.0
 
-    x = 0
+    x  = 0
     th = 0
     status = 0
     cameraUpDownAngle     = 90
     cameraLeftRightAngle  = 90
     gripperUpDownAngle    = 90
     gripperOpenCloseAngle = 75
-
+    
     try:
         print(msg)
         print(vels(speed, turn))
-        while(1):
+
+        while True:
+            x = 0
+            th = 0
             key = getKey()
+            status = (status + 1) % 20
+            if (status == 0):
+                print(msg)
+                
             if key in moveBindings.keys():
                 x = moveBindings[key][0]
                 th = moveBindings[key][1]
@@ -129,68 +129,43 @@ if __name__=="__main__":
                 speed = speed * speedBindings[key][0]
                 turn = turn * speedBindings[key][1]
                 print(vels(speed,turn))
-                if (status == 9):
-                    print(msg)
-                status = (status + 1) % 10
-            elif key in cameraUpBindings.keys():
-                x = 0
-                th = 0
-                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(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(cameraLeftRightAngle))
-                if (cameraLeftRightAngle < 180 and cameraLeftRightAngle >=0):
-                    cameraLeftRightAngle = cameraLeftRightAngle + cameraLeftBindings[key]
-                    client_srv(0, cameraLeftRightAngle)
-            elif key in cameraRightBindings.keys():
-                x = 0
-                th = 0
+            elif key in cameraLeftRightBindings.keys():
                 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
+                if (cameraLeftRightAngle <= 180 and cameraLeftRightAngle >=0):
+                    if(cameraLeftRightAngle == 180 and cameraLeftRightBindings[key] > 0) or\
+                      (cameraLeftRightAngle == 0 and cameraLeftRightBindings[key] < 0):
+                      continue
+                    else:
+                        cameraLeftRightAngle = cameraLeftRightAngle + cameraLeftRightBindings[key]
+                        client_srv(0, cameraLeftRightAngle)
+            elif key in cameraUpDownBindings.keys():
+                print(angles(cameraUpDownAngle))
+                if (cameraUpDownAngle <= 180 and cameraUpDownAngle >= 60):
+                        if (cameraUpDownAngle == 180 and cameraUpDownBindings[key] > 0) or\
+                           (cameraUpDownAngle == 60 and cameraUpDownBindings[key] < 0):
+                            continue
+                        else:
+                            cameraUpDownAngle = cameraUpDownAngle + cameraUpDownBindings[key]
+                            client_srv(1, cameraUpDownAngle)
+            elif key in gripperUpDownBindings.keys():
                 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
+                if (gripperUpDownAngle <= 130 and gripperUpDownAngle >= 65):
+                    if(gripperUpDownAngle == 130 and gripperUpDownBindings[key] > 0) or \
+                        (gripperUpDownAngle == 65 and gripperUpDownBindings[key] < 0):
+                        continue
+                    else:
+                        gripperUpDownAngle = gripperUpDownAngle + gripperUpDownBindings[key]
+                        client_srv(2, gripperUpDownAngle)
+            elif key in gripperOpenCloseBindings.keys():
                 print(angles(gripperOpenCloseAngle))
-                if (gripperOpenCloseAngle < 120 and gripperOpenCloseAngle >=75):
-                    gripperOpenCloseAngle = gripperOpenCloseAngle + gripperCloseBindings[key]
-                    client_srv(3, gripperOpenCloseAngle)
+                if (gripperOpenCloseAngle <= 120 and gripperOpenCloseAngle >= 75):
+                    if(gripperOpenCloseAngle == 120 and gripperOpenCloseBindings[key] > 0) or\
+                      (gripperOpenCloseAngle == 75 and gripperOpenCloseBindings[key] < 0):
+                        continue
+                    else:
+                        gripperOpenCloseAngle = gripperOpenCloseAngle + gripperOpenCloseBindings[key]
+                        client_srv(3, gripperOpenCloseAngle)
             else:
-                x = 0
-                th = 0
                 if (key == '\x03'):
                     break
 
@@ -198,6 +173,7 @@ if __name__=="__main__":
             twist.linear.x = x*speed; twist.linear.y = 0; twist.linear.z = 0;
             twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
             pub.publish(twist)
+            
     except Exception as e:
         print(e)
     finally: