在键盘遥控代码中新增小车夹爪抓取功能

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -1,14 +1,20 @@
 #!/usr/bin/env python
+# coding:utf-8
+
+# Description: 通过键盘遥控小车行进、抓取。
+# History: 
+#   20200317 增加小车抓取功能 by jally
 
 from __future__ import print_function
 
 import roslib; roslib.load_manifest('teleop_twist_keyboard')
 import rospy
-
 from geometry_msgs.msg import Twist
 
 import sys, select, termios, tty
 
+from ros_arduino_msgs.srv import GripperControl
+
 msg = """
 Reading from the keyboard  and Publishing to Twist!
 ---------------------------
@@ -32,6 +38,12 @@ 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%
 
+1 : open the gripper and down
+2 : stop
+3 : close the gripper and up
+4 : open the gripper
+5 : close the gripper
+
 CTRL-C to quit
 """
 
@@ -65,6 +77,14 @@ speedBindings={
         'c':(1,.9),
     }
 
+gripperBindings={
+        '1':(),
+        '2':(),
+        '3':(),
+        '4':(),
+        '5':(),
+    }
+
 def getKey():
     tty.setraw(sys.stdin.fileno())
     select.select([sys.stdin], [], [], 0)
@@ -76,6 +96,18 @@ def getKey():
 def vels(speed,turn):
     return "currently:\tspeed %s\tturn %s " % (speed,turn)
 
+#调用service: mobilebase_arduino/gripper_control
+def client_srv():
+    rospy.wait_for_service("mobilebase_arduino/gripper_control")
+    try:
+        gripper_client = rospy.ServiceProxy("mobilebase_arduino/gripper_control",GripperControl)
+        resp = gripper_client.call(int(key))
+        #rospy.loginfo("Message From server:%s"%resp.feedback)
+    except rospy.ServiceException, e:
+        rospy.logwarn("Service call failed: %s"%e)
+
+
+
 if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
 
@@ -84,6 +116,7 @@ if __name__=="__main__":
 
     speed = rospy.get_param("~speed", 0.12)
     turn = rospy.get_param("~turn", 0.5)
+    
     x = 0
     y = 0
     z = 0
@@ -108,6 +141,8 @@ if __name__=="__main__":
                 if (status == 14):
                     print(msg)
                 status = (status + 1) % 15
+            elif key in gripperBindings.keys():
+                client_srv()
             else:
                 x = 0
                 y = 0