控制180度舵机的转动时,增加了控制速度的参数,就是转动时增加一个延时

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

@@ -18,6 +18,7 @@ from ros_arduino_python.arduino_sensors import *
 from ros_arduino_python.arduino_driver import Arduino
 from ros_arduino_python.base_controller import BaseController
 
+
 class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
@@ -110,10 +111,6 @@ class ArduinoROS():
                 sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
             elif params['type'] == 'BAT_PERCENT':
                 sensor = BatPercent(self.controller, name, params['pin'], params['rate'], self.base_frame)
-            elif params['type'] == 'MotorTotalCurrent':
-                sensor = MotorTotalCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
-            elif params['type'] == 'CurrentValue':
-                sensor = CurrentValue(self.controller, name, params['pin'], params['rate'], self.base_frame)
 
             try:
                 self.mySensors.append(sensor)
@@ -170,12 +167,12 @@ 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):
         value = self.controller.detect_distance()
-        return GetInfraredDistanceResponse(value[0]/100.0,value[1]/100.0,value[2]/100.0)
+        return GetInfraredDistanceResponse(value[0]/100.0, value[1]/100.0, value[2]/100.0)
 
     def DynamicPIDHandler(self, req):
         self.controller.update_pid(req.A_kp, req.A_kd, 0, 10,

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):
-        ''' control gripper angle
+    def gripper_angle(self, servoID, angle, delay):
+        ''' control gripper angle-180 servo
         '''
         #rospy.loginfo("gripper operation ID:" + str(servoID) + ", angle:" + str(angle))
         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\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:
@@ -549,25 +549,6 @@ class Arduino:
             except:
                 return None
 
-    def getCurrentValue(self):
-        '''get current value
-        '''
-        if self.is_use_serial:
-            currentvalue = self.execute('f')
-            #rospy.loginfo("Current value:" + str(currentvalue))
-            return currentvalue
-        else:
-            try:
-                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('f'))
-                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
-                currentvalue = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 5)
-                currentvalue = map(chr, currentvalue)
-                ret = ''.join(currentvalue)
-                #print "ret:"+ret
-                return float(ret)
-            except:
-                return None
-
     def get_pidin(self):
         values = self.execute_array('i')
         if len(values) != 3:
@@ -652,6 +633,7 @@ class Arduino:
             traceback.print_exc(file=sys.stdout)
             return None
 
+
 """ Basic test for connectivity by serial port or IIC bus"""
 if __name__ == "__main__":
     is_use_serial = True

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -102,11 +102,11 @@ def angles(angle):
 
 
 #调用service: mobilebase_arduino/gripper_angle控制夹爪开闭
-def control_180Servo(servoID, angle):
+def control_180Servo(servoID, angle, delay):
     rospy.wait_for_service("mobilebase_arduino/gripper_angle")
     try:
         gripper_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle", GripperAngle)
-        resp = gripper_client.call(servoID, angle)
+        resp = gripper_client.call(servoID, angle, delay)
     except rospy.ServiceException, e:
         rospy.logwarn("Service call failed: %s"%e)
 
@@ -182,7 +182,7 @@ if __name__=="__main__":
                 print(angles(angle))
                 if (angle < 65 and angle >=0):
                     angle = angle + gripperOpenBindings[key]
-                    control_180Servo(1, angle)
+                    control_180Servo(1, angle, 0)
             elif key in gripperCloseBindings.keys():
                 x = 0
                 y = 0
@@ -191,7 +191,7 @@ if __name__=="__main__":
                 print(angles(angle))
                 if (angle <= 65 and angle >0):
                     angle = angle + gripperCloseBindings[key]
-                    control_180Servo(1, angle)
+                    control_180Servo(1, angle, 0)
             else:
                 x = 0
                 y = 0