调试机械臂和夹爪控制,增加控制多个360度舵机的接口

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

@@ -391,7 +391,7 @@ class Arduino:
     def gripper_angle(self, servoID, angle):
         ''' control gripper angle
         '''
-        #rospy.loginfo("gripper operation ID:" + str(operation))
+        #rospy.loginfo("gripper operation ID:" + str(servoID) + ", angle:" + str(angle))
         if self.is_use_serial:
             return self.execute_ack('s %d %d' %(servoID, angle))
         else:

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -39,13 +39,9 @@ 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%
-4/5 : increase/decrease gripper angle by 5°
 
-1 : 钻头上移(同时钻头反转退出)
-2 : 钻头下降(同时钻头正转打孔)
-3 : 运送树苗链条转动
-4 : 放下树苗
-5 : 返回原位
+1/2 : 机械臂上移/下移
+4/5 : 夹爪张开/关闭
 
 CTRL-C to quit
 """
@@ -78,19 +74,16 @@ speedBindings={
         'c':(1,.9),
     }
 
-bitUpBindings={
+armUpBindings={
     '1':(),
     }
-bitDownBindings={
+armDownBindings={
     '2':(),
     }
-chainRotateBindings={
-    '3':(),
-    }
-gripperopenBindings={
+gripperOpenBindings={
     '4':(5),
     }
-grippercloseBindings={
+gripperCloseBindings={
     '5':(-5),
     }
 
@@ -112,8 +105,8 @@ def angles(angle):
 def control_180Servo(servoID, angle):
     rospy.wait_for_service("mobilebase_arduino/gripper_angle")
     try:
-        gripper2_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle", GripperAngle)
-        resp = gripper2_client.call(servoID, angle)
+        gripper_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle", GripperAngle)
+        resp = gripper_client.call(servoID, angle)
     except rospy.ServiceException, e:
         rospy.logwarn("Service call failed: %s"%e)
 
@@ -142,6 +135,7 @@ if __name__=="__main__":
     z = 0
     th = 0
     status = 0
+    angle = 65
 
     try:
         print(msg)
@@ -160,57 +154,44 @@ if __name__=="__main__":
                 if (status == 14):
                     print(msg)
                 status = (status + 1) % 15
-            elif key in bitUpBindings.keys():
+            elif key in armUpBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
                 #print("currently:\tgo up")
-                control_360Servo(0, 3)
-                control_360Servo(1, 1)
+                control_360Servo(0, 1)
                 time.sleep(0.3)
                 #停止舵机转动
                 control_360Servo(0, 2)
-                control_360Servo(1, 2)
-            elif key in bitDownBindings.keys():
+            elif key in armDownBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
                 #print("currently:\tgo down")
-                control_360Servo(0, 1)
-                control_360Servo(1, 3)
+                control_360Servo(0, 3)
                 time.sleep(0.3)
                 #停止舵机转动
                 control_360Servo(0, 2)
-                control_360Servo(1, 2)
-            elif key in chainRotateBindings.keys():
-                x = 0
-                y = 0
-                z = 0
-                th = 0
-                control_360Servo(2, 1)
-                time.sleep(0.3)
-                #停止舵机转动
-                control_360Servo(1, 2)
-            elif key in gripperopenBindings.keys():
+            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():
+                    angle = angle + gripperOpenBindings[key]
+                    control_180Servo(1, angle)
+            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()
+                    angle = angle + gripperCloseBindings[key]
+                    control_180Servo(1, angle)
             else:
                 x = 0
                 y = 0