更新control_robot.py控制机器人移动的代码,兼容了多机器人编队时控制命令,只需要在.bashrc中配置ROBOT_NS变量即可

src/control_robot/control_robot.py

@@ -10,7 +10,7 @@
 #    20220224:更新软件包名,代码文件名.
 
 from __future__ import print_function
-import rospy, roslib
+import os, rospy, roslib
 import sys,select,termios,tty
 from geometry_msgs.msg import Twist
 from ros_arduino_msgs.srv import ServoWrite
@@ -86,17 +86,21 @@ def angles(angle):
 
 #调用service: mobilebase_arduino/servo_write控制对应舵机转动
 def client_srv(ser_id, ser_angle):
-    rospy.wait_for_service("mobilebase_arduino/servo_write")
+    robot_ns = os.environ.get('ROBOT_NS')
+    service_name = robot_ns + "/" + "mobilebase_arduino/servo_write"
+    rospy.wait_for_service(service_name)
     try:
-        camera_client = rospy.ServiceProxy("mobilebase_arduino/servo_write", ServoWrite)
+        camera_client = rospy.ServiceProxy(service_name, ServoWrite)
         resp = camera_client.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)
+    robot_ns = os.environ.get('ROBOT_NS')
+    cmd_name = robot_ns + "/" + "cmd_vel"
 
-    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
+    pub = rospy.Publisher(cmd_name, Twist, queue_size = 1)
     rospy.init_node('control_robot_node')
 
     #初始化控制命令参数,线速度0.2m/s，角速度1.0rad/s