Merge remote-tracking branch 'origin/treeRobot'

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperAngle.srv

@@ -1,2 +1,3 @@
-float32 angle
+int32 servoID
+int32 angle
 ---

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperControl.srv

@@ -1,2 +1,3 @@
-float32 value
+int32 servoID
+int32 value
 ---

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -166,11 +166,11 @@ class ArduinoROS():
         return LightShowResponse()
 
     def GripperControlHandler(self, req):
-        self.controller.gripper_control(req.value)
+        self.controller.gripper_control(req.servoID, req.value)
         return GripperControlResponse()
 
     def GripperAngleHandler(self, req):
-        self.controller.gripper_angle(req.angle)
+        self.controller.gripper_angle(req.servoID, req.angle)
         return GripperAngleResponse()
 
     def GetInfraredDistanceHandler(self, req):

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

@@ -373,31 +373,31 @@ class Arduino:
         except:
             return None
 
-    def gripper_control(self, operation):
+    def gripper_control(self, servoID, operation):
         ''' control gripper: 1:down 2:stop 3:up
         '''
         #rospy.loginfo("gripper operation ID:" + str(operation))
         if self.is_use_serial:
-            return self.execute_ack('z %d' %(operation))
+            return self.execute_ack('z %d %d' %(servoID, operation))
         else:
             '''control gripper up/down by IIC
             '''
-            cmd = (' %d\r' %(operation))
+            cmd = (' %d %d\r' %(servoID, operation))
             try:
                 self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('z'), [ord(c) for c in cmd])
             except:
                 return None
 
-    def gripper_angle(self, angle):
+    def gripper_angle(self, servoID, angle):
         ''' control gripper angle
         '''
         #rospy.loginfo("gripper operation ID:" + str(operation))
         if self.is_use_serial:
-            return self.execute_ack('s 1 %d' %(angle))
+            return self.execute_ack('s %d %d' %(servoID, angle))
         else:
             '''control gripper open/close by IIC
             '''
-            cmd = (' 1 %d\r' %(angle))
+            cmd = (' %d %d\r' %(servoID, angle))
             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

@@ -34,9 +34,6 @@ For Holonomic mode (strafing), hold down the shift key:
    J    K    L
    M    <    >
 
-t : up (+z)
-b : down (-z)
-
 anything else : stop
 
 q/z : increase/decrease max speeds by 10%
@@ -44,10 +41,11 @@ 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 : arm go up
-2 : arm go dowm
-4 : open the gripper
-5 : close the gripper
+1 : 钻头上移(同时钻头反转退出)
+2 : 钻头下降(同时钻头正转打孔)
+3 : 运送树苗链条转动
+4 : 放下树苗
+5 : 返回原位
 
 CTRL-C to quit
 """
@@ -69,8 +67,6 @@ moveBindings = {
         '<':(-1,0,0,0),
         '>':(-1,-1,0,0),
         'M':(-1,1,0,0),
-        't':(0,0,1,0),
-        'b':(0,0,-1,0),
     }
 
 speedBindings={
@@ -82,17 +78,20 @@ speedBindings={
         'c':(1,.9),
     }
 
-gripperupBindings={
-        '1':(),
+bitUpBindings={
+    '1':(),
+    }
+bitDownBindings={
+    '2':(),
     }
-gripperdownBindings={
-        '2':(),
+chainRotateBindings={
+    '3':(),
     }
 gripperopenBindings={
-        '4':(5),
+    '4':(5),
     }
 grippercloseBindings={
-        '5':(-5),
+    '5':(-5),
     }
 
 def getKey():
@@ -102,32 +101,37 @@ def getKey():
     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
     return key
 
-
-def vels(speed,turn):
+def vels(speed, turn):
     return "currently:\tspeed %s\tturn %s " % (speed,turn)
 
 def angles(angle):
     return "currently:\tangle %s " % (angle)
 
 
-#调用service: mobilebase_arduino/gripper_control控制夹爪升降
 #调用service: mobilebase_arduino/gripper_angle控制夹爪开闭
-def client_srv():
-    rospy.wait_for_service("mobilebase_arduino/gripper_control")
+def control_180Servo(servoID, angle):
     rospy.wait_for_service("mobilebase_arduino/gripper_angle")
     try:
-        gripper_client = rospy.ServiceProxy("mobilebase_arduino/gripper_control",GripperControl)
-        gripper2_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle",GripperAngle)
-        resp = gripper_client.call(command)
-        resp = gripper2_client.call(angle)
+        gripper2_client = rospy.ServiceProxy("mobilebase_arduino/gripper_angle", GripperAngle)
+        resp = gripper2_client.call(servoID, angle)
     except rospy.ServiceException, e:
         rospy.logwarn("Service call failed: %s"%e)
 
 
+#调用service: mobilebase_arduino/gripper_control控制360度舵机转动
+def control_360Servo(servoID, cmd):
+    rospy.wait_for_service("mobilebase_arduino/gripper_control")
+    try:
+        client = rospy.ServiceProxy("mobilebase_arduino/gripper_control", GripperControl)
+        client.call(servoID, cmd)
+    except rospy.ServiceException, e:
+        rospy.logwarn("Service call failed: %s" %e)
+
+
 if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
 
-    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 5)
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 2)
     rospy.init_node('teleop_twist_keyboard')
 
     speed = rospy.get_param("~speed", 0.2)
@@ -138,8 +142,6 @@ if __name__=="__main__":
     z = 0
     th = 0
     status = 0
-    angle = 10
-    command = 2
 
     try:
         print(msg)
@@ -154,35 +156,43 @@ if __name__=="__main__":
             elif key in speedBindings.keys():
                 speed = speed * speedBindings[key][0]
                 turn = turn * speedBindings[key][1]
-
                 print(vels(speed,turn))
                 if (status == 14):
                     print(msg)
-                status = (status + 1) % 15           
-            elif key in gripperupBindings.keys():
+                status = (status + 1) % 15
+            elif key in bitUpBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
-                command = 3
                 #print("currently:\tgo up")
-                client_srv()
+                control_360Servo(0, 3)
+                control_360Servo(1, 1)
                 time.sleep(0.3)
-                command = 2
-                #print("currently:\tstop !")
-                client_srv()          
-            elif key in gripperdownBindings.keys():
+                #停止舵机转动
+                control_360Servo(0, 2)
+                control_360Servo(1, 2)
+            elif key in bitDownBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
-                command = 1
                 #print("currently:\tgo down")
-                client_srv()
+                control_360Servo(0, 1)
+                control_360Servo(1, 3)
                 time.sleep(0.3)
-                command = 2
-                #print("currently:\tstop !")
-                client_srv()
+                #停止舵机转动
+                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():
                 x = 0
                 y = 0
@@ -200,7 +210,7 @@ if __name__=="__main__":
                 print(angles(angle))
                 if (angle <= 65 and angle >0):
                     angle = angle + grippercloseBindings[key]
-                    client_srv()         
+                    client_srv()
             else:
                 x = 0
                 y = 0
@@ -213,10 +223,8 @@ if __name__=="__main__":
             twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
             twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
             pub.publish(twist)
-
     except Exception as e:
         print(e)
-
     finally:
         twist = Twist()
         twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0