增加控制机器人集群的代码control_robot_team.py

src/control_robot/control_robot_team.py

@@ -0,0 +1,195 @@
+#!/usr/bin/env python
+# coding:utf-8
+
+# Copyright: 2016-2024 wwww.corvin.cn ROS小课堂
+# Description: 通过键盘遥控小车行进，摄像头舵机转动,2DOF机械臂转动.
+# History:
+#    20240106:控制机器人集群运动.
+
+from __future__ import print_function
+import rospy, roslib
+import sys,select,termios,tty
+from geometry_msgs.msg import Twist
+from ros_arduino_msgs.srv import ServoWrite
+
+msg = """
+使用键盘控制小车的夹爪和移动,控制按键如下:
+------------------------------
+     (左前) (前进) (右前)
+       u      i      o
+(左转) j      k      l (右转)
+       m      <      >
+     (左后) (后退) (右后)
+------------------------------
+q/z : 每次按照10%同比例来增加或减小线速度和角速度
+w/x : 每次按照10%比例来增加或减小线速度
+e/c : 每次按照10%比例来增加或减小角速度
+1/2 : 相机向上/向下转动
+3/4 : 相机向左/向右转动
+5/6 : 夹爪向上/向下转动
+7/8 : 夹爪打开/关闭操作
+-------------------------------
+Ctrl+c: 退出键盘控制模式
+"""
+
+moveBindings = {
+        'i':(1,0),
+        'j':(0,1),
+        'l':(0,-1),
+        ',':(-1,0),
+        'u':(1,1),
+        'o':(1,-1),
+        'm':(-1,-1),
+        '.':(-1,1),
+    }
+speedBindings={
+        'q':(1.1,1.1),
+        'z':(0.9,0.9),
+        'w':(1.1,1),
+        'x':(0.9,1),
+        'e':(1,1.1),
+        'c':(1,0.9),
+    }
+cameraUpDownBindings={
+        '1':(-5),
+        '2':(5),
+    }
+cameraLeftRightBindings={
+        '3':(5),
+        '4':(-5),
+    }
+gripperUpDownBindings={
+        '5':(5),
+        '6':(-5),
+    }
+gripperOpenCloseBindings={
+        '7':(-5),
+        '8':(5),
+    }
+
+#等待键盘按键被按下,获取到按键后将其返回
+def getKey():
+    tty.setraw(sys.stdin.fileno())
+    select.select([sys.stdin], [], [], 0)
+    key = sys.stdin.read(1)
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    return key
+
+def vels(speed, turn):
+    return "当前控制速度: 线速度: %s m/s 转弯角速度: %s rad/s" %(speed,turn)
+
+def angles(angle):
+    return "当前角度:  %s " % (angle)
+
+#调用service: mobilebase_arduino/servo_write控制对应舵机转动
+def client_srv(ser_id, ser_angle):
+    rospy.wait_for_service("/panda_1/mobilebase_arduino/servo_write")
+    rospy.wait_for_service("/panda_2/mobilebase_arduino/servo_write")
+    rospy.wait_for_service("/panda_3/mobilebase_arduino/servo_write")
+    try:
+        camera_client_1 = rospy.ServiceProxy("/panda_1/mobilebase_arduino/servo_write", ServoWrite)
+        camera_client_2 = rospy.ServiceProxy("/panda_2/mobilebase_arduino/servo_write", ServoWrite)
+        camera_client_3 = rospy.ServiceProxy("/panda_3/mobilebase_arduino/servo_write", ServoWrite)
+        resp = camera_client_1.call(ser_id, ser_angle)
+        resp = camera_client_2.call(ser_id, ser_angle)
+        resp = camera_client_3.call(ser_id, ser_angle)
+    except (rospy.ServiceException, rospy.ROSException) as e:
+        rospy.logwarn("Service call failed: %s"%e)
+
+if __name__=="__main__":
+    settings = termios.tcgetattr(sys.stdin)
+
+    pub_1 = rospy.Publisher('/panda_1/cmd_vel', Twist, queue_size = 1)
+    pub_2 = rospy.Publisher('/panda_2/cmd_vel', Twist, queue_size = 1)
+    pub_3 = rospy.Publisher('/panda_3/cmd_vel', Twist, queue_size = 1)
+    rospy.init_node('control_robot_node')
+
+    #初始化控制命令参数,线速度0.2m/s，角速度1.0rad/s
+    speed = 0.2
+    turn = 1.0
+
+    x  = 0
+    th = 0
+    status = 0
+    cameraUpDownAngle     = 90
+    cameraLeftRightAngle  = 90
+    gripperUpDownAngle    = 90
+    gripperOpenCloseAngle = 75
+
+    try:
+        print(msg)
+        print(vels(speed, turn))
+
+        while True:
+            x = 0
+            th = 0
+            key = getKey()
+            status = (status + 1) % 20
+            if (status == 0):
+                print(msg)
+
+            if key in moveBindings.keys():
+                x = moveBindings[key][0]
+                th = moveBindings[key][1]
+            elif key in speedBindings.keys():
+                speed = speed * speedBindings[key][0]
+                turn = turn * speedBindings[key][1]
+                print(vels(speed,turn))
+            elif key in cameraLeftRightBindings.keys():
+                print(angles(cameraLeftRightAngle))
+                if (cameraLeftRightAngle <= 180 and cameraLeftRightAngle >=0):
+                    if(cameraLeftRightAngle == 180 and cameraLeftRightBindings[key] > 0) or\
+                      (cameraLeftRightAngle == 0 and cameraLeftRightBindings[key] < 0):
+                      continue
+                    else:
+                        cameraLeftRightAngle = cameraLeftRightAngle + cameraLeftRightBindings[key]
+                        client_srv(0, cameraLeftRightAngle)
+            elif key in cameraUpDownBindings.keys():
+                print(angles(cameraUpDownAngle))
+                if (cameraUpDownAngle <= 180 and cameraUpDownAngle >= 60):
+                        if (cameraUpDownAngle == 180 and cameraUpDownBindings[key] > 0) or\
+                           (cameraUpDownAngle == 60 and cameraUpDownBindings[key] < 0):
+                            continue
+                        else:
+                            cameraUpDownAngle = cameraUpDownAngle + cameraUpDownBindings[key]
+                            client_srv(1, cameraUpDownAngle)
+            elif key in gripperUpDownBindings.keys():
+                print(angles(gripperUpDownAngle))
+                if (gripperUpDownAngle <= 130 and gripperUpDownAngle >= 65):
+                    if(gripperUpDownAngle == 130 and gripperUpDownBindings[key] > 0) or \
+                        (gripperUpDownAngle == 65 and gripperUpDownBindings[key] < 0):
+                        continue
+                    else:
+                        gripperUpDownAngle = gripperUpDownAngle + gripperUpDownBindings[key]
+                        client_srv(2, gripperUpDownAngle)
+            elif key in gripperOpenCloseBindings.keys():
+                print(angles(gripperOpenCloseAngle))
+                if (gripperOpenCloseAngle <= 120 and gripperOpenCloseAngle >= 75):
+                    if(gripperOpenCloseAngle == 120 and gripperOpenCloseBindings[key] > 0) or\
+                      (gripperOpenCloseAngle == 75 and gripperOpenCloseBindings[key] < 0):
+                        continue
+                    else:
+                        gripperOpenCloseAngle = gripperOpenCloseAngle + gripperOpenCloseBindings[key]
+                        client_srv(3, gripperOpenCloseAngle)
+            else:
+                if (key == '\x03'):
+                    break
+
+            twist = Twist()
+            twist.linear.x = x*speed; twist.linear.y = 0; twist.linear.z = 0;
+            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
+            pub_1.publish(twist)
+            pub_2.publish(twist)
+            pub_3.publish(twist)
+    except Exception as e:
+        print(e)
+    finally:
+        twist = Twist()
+        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
+        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
+        pub_1.publish(twist)
+        pub_2.publish(twist)
+        pub_3.publish(twist)
+
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+