摇臂舵机放苗时，增加可以控制舵机转动速度参数

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperAngle.srv

@@ -1,3 +1,4 @@
 int32 servoID
 int32 angle
+int32 delay
 ---

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -170,7 +170,7 @@ class ArduinoROS():
         return GripperControlResponse()
 
     def GripperAngleHandler(self, req):
-        self.controller.gripper_angle(req.servoID, req.angle)
+        self.controller.gripper_angle(req.servoID, req.angle, req.delay)
         return GripperAngleResponse()
 
     def GetInfraredDistanceHandler(self, req):

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

@@ -388,16 +388,16 @@ class Arduino:
             except:
                 return None
 
-    def gripper_angle(self, servoID, angle):
+    def gripper_angle(self, servoID, angle, delay):
         ''' control gripper angle
         '''
         #rospy.loginfo("gripper operation ID:" + str(operation))
         if self.is_use_serial:
-            return self.execute_ack('s %d %d' %(servoID, angle))
+            return self.execute_ack('s %d %d %d' %(servoID, angle, delay))
         else:
             '''control gripper open/close by IIC
             '''
-            cmd = (' %d %d\r' %(servoID, angle))
+            cmd = (' %d %d %d\r' %(servoID, angle, delay))
             try:
                 self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('s'), [ord(c) for c in cmd])
             except:

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -39,13 +39,14 @@ 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 : 返回原位
+3 : 运送树苗链条往外转动
+4 : 运送树苗链条往里转动
+5 : 移动一颗树苗下来
+6 : 摇臂放下树苗
+7 : 摇臂返回原位
 
 CTRL-C to quit
 """
@@ -87,11 +88,17 @@ bitDownBindings={
 chainRotateBindings={
     '3':(),
     }
+chainRotateBackBindings={
+    '4':(),
+    }
+PutOneTreeBindings={
+    '5':(),
+    }
 putDownBindings={
-    '4':(5),
+    '6':(5),
     }
 returnBindings={
-    '5':(-5),
+    '7':(-5),
     }
 
 def getKey():
@@ -104,12 +111,12 @@ def getKey():
 def vels(speed, turn):
     return "currently:\tspeed %s\tturn %s " % (speed,turn)
 
-#调用service: mobilebase_arduino/gripper_angle控制夹爪开闭
-def control_180Servo(servoID, angle):
+#调用service: mobilebase_arduino/gripper_angle控制摇臂舵机
+def control_180Servo(servoID, angle, delay):
     rospy.wait_for_service("mobilebase_arduino/gripper_angle")
     try:
         gripper2_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle", GripperAngle)
-        resp = gripper2_client.call(servoID, angle)
+        resp = gripper2_client.call(servoID, angle, delay)
     except rospy.ServiceException, e:
         rospy.logwarn("Service call failed: %s"%e)
 
@@ -180,7 +187,7 @@ if __name__=="__main__":
                 #停止舵机转动
                 control_360Servo(0, 2)
                 control_360Servo(1, 2)
-            elif key in chainRotateBindings.keys():
+            elif key in chainRotateBindings.keys():#树苗仓往外移动
                 x = 0
                 y = 0
                 z = 0
@@ -189,18 +196,36 @@ if __name__=="__main__":
                 time.sleep(0.05)
                 #停止舵机转动
                 control_360Servo(2, 2)
+            elif key in chainRotateBackBindings.keys():#树苗仓往里移动
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                control_360Servo(2, 1)
+                time.sleep(0.05)
+                #停止舵机转动
+                control_360Servo(2, 2)
+            elif key in PutOneTreeBindings.keys(): #运送一颗树苗掉落
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                control_360Servo(2, 3)
+                time.sleep(0.65)
+                #停止舵机转动
+                control_360Servo(2, 2)
             elif key in putDownBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
-                control_180Servo(3, 0)
+                control_180Servo(3, 0, 10)
             elif key in returnBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
-                control_180Servo(3, 160)
+                control_180Servo(3, 165, 10)
             else:
                 x = 0
                 y = 0