Merge branch 'master' of code.corvin.cn:corvin_zhang/blackTornadoRobot

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -22,6 +22,7 @@ add_service_files(FILES
                   AnalogRead.srv
                   AlarmWrite.srv
                   LightShow.srv
+                  GripperControl.srv
                  )
 
 generate_messages(   

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperControl.srv

@@ -0,0 +1,2 @@
+float32 value
+---

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -64,6 +64,9 @@ class ArduinoROS():
         # A service to set light show
         rospy.Service('~light_show', LightShow, self.LightShowHandler)
 
+        # A service to position PWM servo
+        rospy.Service('~gripper_control', GripperControl, self.GripperControlHandler)
+
         # Initialize the controlller
         self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
@@ -147,6 +150,10 @@ class ArduinoROS():
         self.controller.light_show(req.value)
         return LightShowResponse()
 
+    def GripperControlHandler(self, req):
+        self.controller.gripper_control(req.value)
+        return GripperControlResponse()
+
     # Stop the robot
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")

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

@@ -6,15 +6,11 @@
 """
 
 from math import pi as PI
-import sys, traceback
 from serial.serialutil import SerialException
+import thread, smbus, rospy, time, os
 from serial import Serial
+import sys, traceback
 import numpy as np
-import thread
-import smbus
-import rospy
-import time
-import os
 
 
 class Arduino:
@@ -28,7 +24,6 @@ class Arduino:
         self.PID_INTERVAL = 25
 
         self.is_use_serial = is_use_serial #与下位机arduino的通信方式是否使用串口还是IIC
-
         self.serial_port = serial_port
         self.baudrate    = baudrate
 
@@ -353,6 +348,14 @@ class Arduino:
         except:
             return None
 
+    def gripper_control(self, operation):
+        ''' control gripper: 1-open gripper down 2:stop 3:close gripper up 4:open gripper 5:close gripper
+        '''
+        #rospy.loginfo("gripper operation ID:" + str(operation))
+        return self.execute_ack('z %d' %(operation))
+
+    
+
     def drive(self, AWheel, BWheel, CWheel):
         ''' Speeds are given in encoder ticks per PID interval
         '''