更新机器人编队控制代码

src/control_robot/control_robot_team.py

@@ -13,7 +13,7 @@ from geometry_msgs.msg import Twist
 from ros_arduino_msgs.srv import ServoWrite
 
 msg = """
-使用键盘控制小车的夹爪和移动,控制按键如下:
+使用键盘控制小车编队,控制按键如下:
 ------------------------------
      (左前) (前进) (右前)
        u      i      o
@@ -86,13 +86,22 @@ 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")
+    rospy.wait_for_service("/panda_4/mobilebase_arduino/servo_write")
+    rospy.wait_for_service("/panda_5/mobilebase_arduino/servo_write")
+    rospy.wait_for_service("/panda_6/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)
+        camera_client_4 = rospy.ServiceProxy("/panda_4/mobilebase_arduino/servo_write", ServoWrite)
+        camera_client_5 = rospy.ServiceProxy("/panda_5/mobilebase_arduino/servo_write", ServoWrite)
+        camera_client_6 = rospy.ServiceProxy("/panda_6/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)
+        resp = camera_client_4.call(ser_id, ser_angle)
+        resp = camera_client_5.call(ser_id, ser_angle)
+        resp = camera_client_6.call(ser_id, ser_angle)
     except (rospy.ServiceException, rospy.ROSException) as e:
         rospy.logwarn("Service call failed: %s"%e)
 
@@ -102,6 +111,9 @@ if __name__=="__main__":
     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)
+    pub_4 = rospy.Publisher('/panda_4/cmd_vel', Twist, queue_size = 1)
+    pub_5 = rospy.Publisher('/panda_5/cmd_vel', Twist, queue_size = 1)
+    pub_6 = rospy.Publisher('/panda_6/cmd_vel', Twist, queue_size = 1)
     rospy.init_node('control_robot_node')
 
     #初始化控制命令参数,线速度0.2m/s，角速度1.0rad/s
@@ -181,6 +193,9 @@ if __name__=="__main__":
             pub_1.publish(twist)
             pub_2.publish(twist)
             pub_3.publish(twist)
+            pub_4.publish(twist)
+            pub_5.publish(twist)
+            pub_6.publish(twist)
     except Exception as e:
         print(e)
     finally:
@@ -190,6 +205,9 @@ if __name__=="__main__":
         pub_1.publish(twist)
         pub_2.publish(twist)
         pub_3.publish(twist)
+        pub_4.publish(twist)
+        pub_5.publish(twist)
+        pub_6.publish(twist)
 
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)