1

hotwheelspkg/scripts/calculate.py

+import math, sympy
+from numpy import angle
+# 计算单位: cm
+L1 = 8.0000             # 大腿长
+L2 = 15.321             # 小腿长
+X0 = 15                 # 初始腿的末端到舵机根的距离
+# 定义初始位置满足的关系
+# L1*cos(base1)+L2*cos(base2)=x0
+# L1*sin(base1)+L2*sin(base2)=0
+base1 = 1.3427          #arccos((L1**2+X0**2-L2**2)/(2*X0*L1))
+base2 = -0.5336         #base1代入方程反解
+
+def calc_turn(input_angle):
+    # input_angle 实际转过的角度， 返回舵机转的角度
+    input_angle = input_angle / 180 * 3.1415926
+    temp = math.asin(8 / 11 * math.sin(input_angle)) + input_angle
+    res = int(-temp / 3.1415926 * 180)
+    # print("SdjakdjakljdklaJdljadk: ", res)
+    return res
+    # return 2 * math.atan(11*math.sin(input_angle)/(11*math.cos(input_angle)+8))  / 3.1415926 * 180
+
+#小腿153.21, 大腿80
+def calc_pos(angle1, angle2):
+    rad1 = angle1 / 180 * 3.1415926
+    rad2 = angle2 / 180 * 3.1415926
+    x = L1 * math.cos(rad1 + base1) + L2 * math.cos(rad1 + rad2 + base2)
+    y = L1 * math.sin(rad1 + base1) + L2 * math.sin(rad1 + rad2 + base2)
+    return x, y
+
+#temp1 = (L2*L2-L1*L1 - (x**2+y**2)) / 2*L1
+#temp2 = (L2*L2-L1*L1 + (x**2+y**2)) / 2*L2
+def calc_angle(x, y):    
+    # x*sin(r1)+y*cos(r1)=sqrt(x**2+y**2)sin(r1+atan(y/x))
+    # 两个常数分别为L2**2-L1**2, 2*L1
+    square = x**2 + y**2
+    temp1 = (square - (L2 * L2 - L1 * L1)) / (2 * L1)   
+    # 辅助角
+    at = math.pi/2 if x == 0 else math.atan(y/x)
+    rad1 = math.acos(temp1 / math.sqrt(square)) + at - base1
+    angle1 = int(rad1 / 3.1415926 * 180)
+    temp2 = (square + (L2 * L2 - L1 * L1)) / (2 * L2)
+    rad2 = -math.acos(temp2 / math.sqrt(square)) + at - rad1 - base2
+    angle2 = int(rad2 / 3.1415926 * 180)
+
+    flag = 1
+    # if(angle2 > 90):
+    #     angle2 = angle2 - 180 
+    return flag, angle1, angle2
+
+if __name__ == "__main__":
+    # pass
+    print(calc_turn(-17))
+    # print(int(calc_turn(20) / 3.1415926 * 180))
+    # print(int(calc_turn(30) / 3.1415926 * 180))
+    # print(int(calc_turn(40) / 3.1415926 * 180))
+    # print(int(calc_turn(50) / 3.1415926 * 180))
+    # x, y = calc_pos(28, -27)
+    # print(x, y)
+    # _, angle1, angle2 = calc_angle(x, y)
+    # print(angle1, angle2)
+    # print(calc_pos(-46, 17))
+    # print(calc_pos(angle1, angle2))
+    # print(calc_pos(0, 0))
+    # print(calc_pos(-29, -14))

hotwheelspkg/scripts/camera_cv.py

+import cv2
+import rospy
+from sensor_msgs.msg import CameraInfo, Image
+from std_msgs.msg import String
+from cv_bridge import CvBridge
+import numpy as np
+import math
+
+
+def find_color(image, color='blue'):
+    '''
+    寻找色块
+    '''
+    color_dist = {
+        'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
+        'blue': {'Lower': np.array([97, 224, 70]), 'Upper': np.array([124, 255, 255])},
+        'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
+    }
+    gs_frame = cv2.GaussianBlur(image, (5, 5), 0)                     # 高斯模糊
+    hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像
+    erode_hsv = cv2.erode(hsv, None, iterations=2)                   # 腐蚀 粗的变细
+    inRange_hsv = cv2.inRange(erode_hsv, color_dist[color]['Lower'], color_dist[color]['Upper'])    #识别颜色
+    cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]     #寻找轮廓
+    try:
+        c = max(cnts, key=cv2.contourArea)                              #寻找最大色块
+    except:
+        return 0
+    rect = cv2.minAreaRect(c)                                       #找最小的包裹色块的矩形
+    box = cv2.boxPoints(rect)                                       #box的点
+    sumx = 0.0
+    sumy = 0.0
+    for pi in box:
+        sumx += pi[0]
+        sumy += pi[1]
+    center = (int(sumx / 4), int(sumy / 4))
+    sita = math.atan(math.tan(34.5 / 180 * 3.1415926) * (center[0] - 320) / 320) / 3.1415926 * 180
+    # print("center, sita = ", center, sita)
+    # cv2.drawContours(image, [np.int0(box)], -1, (0, 255, 255), 2)
+    # cv2.circle(image, center, 7, (0, 0, 0), -1)
+    return sita
+
+def image_process(msg):
+    image = CvBridge().imgmsg_to_cv2(msg, "bgr8")
+    sita = find_color(image)
+    # cv2.imshow('image', new_image)
+    # cv2.waitKey(1)
+    return sita
+    # cv2.imwrite('1.jpg', image)
+
+# 目标所在未知在图片中的角度
+# def get_angle():
+#     rospy.spinOnce()
+#     return .sita
+
+def depth_process(msg):
+    image = CvBridge().imgmsg_to_cv2(msg, '16UC1')
+    image_arr = np.array(image)
+    # print(image_arr.shape[0])
+    divide_p = [image_arr.shape[1] // 4, image_arr.shape[1] // 2, image_arr.shape[1] * 3 // 4]
+    center_part = image_arr[int(image_arr.shape[0] / 2):, divide_p[0]:divide_p[2]]
+    left_part = image_arr[int(image_arr.shape[0] / 2):, 0:divide_p[1]]
+    right_part = image_arr[int(image_arr.shape[0] / 2):, divide_p[1]:image_arr.shape[1]]
+    center_part_mean = center_part.mean()
+    left_part_mean = left_part.mean()
+    right_part_mean = right_part.mean()
+    return center_part_mean, left_part_mean, right_part_mean
+
+def depth_callback(msg):
+    print(depth_process(msg))
+    image = CvBridge().imgmsg_to_cv2(msg, '16UC1')
+    image -= image.min()
+    image = image / (image.max() - image.min())
+    image *= 255
+    # image_arr = np.array(image) 
+    # print(image_arr)
+    cv2.imwrite('2.jpg', image)
+    # np.savetxt( "a.csv", image_arr, delimiter=",")
+
+
+if __name__ == "__main__":
+    rospy.init_node("camera")
+
+    # sub = rospy.Subscriber("/camera/color/image_raw", Image, image_process)
+    depth = rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, depth_callback)
+
+    rate = rospy.Rate(1000)
+
+    rospy.spin()
+    rate.sleep()
\ No newline at end of file

hotwheelspkg/scripts/controller.py

+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+import time
+import cmd
+import logging
+
+def SendCommand(msg):
+    print(msg.data)
+    pub.publish(msg)
+
+class CommandCLI(cmd.Cmd):
+    intro = 'XLAB机器人控制平台, 输入 help 或者?查看帮助。\n'
+    prompt = '> 请输入指令: '
+    msg = String()
+
+    def ErrorArgs(self):
+        print('参数个数错误，请检查输入')
+
+    def checkArgs(self, arg, aim):
+        '检查参数合法性; arg1:输入参数; arg2:包含符合要求的参数个数'
+        args = arg.split(' ')
+        argnum = 0 if arg == '' else len(args)
+        invalid = False
+        if(argnum not in aim):
+            self.ErrorArgs()
+            invalid = True
+        return args, argnum, invalid
+
+    def default(self, line):
+        print('无效指令，请检查输入')
+
+    def precmd(self, line: str) -> str:
+        logger.info(f'{line}')
+        return super().precmd(line)
+
+    def do_EOF(self, line):
+        '退出快捷键, 输入ctrl+D发送EOF使得控制终端退出'
+        return True
+
+    def do_scan(self, arg):
+        '扫描舵机'
+        args, argnum, invalid = self.checkArgs(arg, [0])
+        if invalid: return 
+        self.msg.data = 'manager.' + 'ServoScan()'
+        SendCommand(self.msg)
+    
+    def do_rst(self, arg):
+        '复位至初始位置'
+        args, argnum, invalid = self.checkArgs(arg, [0])
+        if invalid: return 
+        self.msg.data = 'manager.' + 'Init()'
+        SendCommand(self.msg)
+    
+    def do_set(self, arg):
+        '设置初始位置'
+        args, argnum, invalid = self.checkArgs(arg, [0])
+        if invalid: return 
+        self.msg.data = 'manager.' + 'SetInit()'
+        SendCommand(self.msg)
+    
+    def do_damp(self, arg):
+        '使所有舵机转变为低阻尼模式'
+        args, argnum, invalid = self.checkArgs(arg, [0, 1])
+        if invalid: return 
+        if(argnum == 0):
+            self.msg.data = 'manager.' + 'DampAll()'
+        else:
+            self.msg.data = 'manager.' + f'Damp({args[0]})'
+        SendCommand(self.msg)
+    
+    def do_show(self, arg):
+        '显示舵机状态'
+        args = arg.split(' ')
+        if(len(args) > 1): 
+            self.ErrorArgs()
+            return
+        elif(arg == ''):
+            self.msg.data = 'manager.' + f'ShowServoState()'
+        else:
+            self.msg.data = 'manager.' + f'ShowServoState({args[0]})'
+        SendCommand(self.msg)
+    
+    def do_showabs(self, arg):
+        '显示舵机状态'
+        args = arg.split(' ')
+        if(len(args) > 1): 
+            self.ErrorArgs()
+            return
+        elif(arg == ''):
+            self.msg.data = 'manager.' + f'ShowServoAbsState()'
+        else:
+            self.msg.data = 'manager.' + f'ShowServoAbsState({args[0]})'
+        SendCommand(self.msg)
+    
+    def do_showf(self, arg):
+        '显示文件; arg1: 子目录名; arg2: 文件名'
+        args = arg.split(' ')
+        if(len(args) != 2): 
+            self.ErrorArgs()
+            return
+        self.msg.data = 'manager.' + f'ShowFile("{args[0]}", "{args[1]}")'
+        SendCommand(self.msg)
+    
+    def do_wt(self, arg):
+        '保存时间流; arg1: 时间流文件名; arg2: 时间流时长;'
+        args = arg.split(' ')
+        if(len(args) != 2): 
+            self.ErrorArgs()
+            return
+        self.msg.data = 'manager.' + f'SaveServoFlow("{args[0]}", {args[1]}, "TimeFlow")'
+        SendCommand(self.msg)
+
+    def do_rt(self, arg):
+        '加载时间流; arg1: 时间流文件名;'
+        args = arg.split(' ')
+        if(len(args) != 1): 
+            self.ErrorArgs()
+            return
+        self.msg.data = 'manager.' + f'LoadServoFlow("{args[0]}", "TimeFlow")'
+        SendCommand(self.msg)
+    
+    def do_ws(self, arg):
+        '保存单步动作; arg1: 文件名; arg2: 单步动作序号'
+        args = arg.split(' ')
+        if(len(args) != 2): 
+            self.ErrorArgs()
+            return
+        self.msg.data = 'manager.' + f'SaveServo("{args[0]}", "Step", {args[1]})'
+        SendCommand(self.msg)
+    
+    def do_ra(self, arg):
+        '读取action; arg1: action名'
+        args, argnum, invalid = self.checkArgs(arg, [1])
+        if invalid: return 
+        self.msg.data = 'manager.' + f'ReadAction("{args[0]}")'
+        SendCommand(self.msg)
+    
+    def do_wa(self, arg):
+        '记录action; arg1: action名'
+        args, argnum, invalid = self.checkArgs(arg, [1])
+        if invalid: return 
+        self.msg.data = 'manager.' + f'WriteAction("{args[0]}")'
+        SendCommand(self.msg)
+
+    def do_rs(self, arg):
+        '加载单步动作; arg1: 文件名; arg2: 单步动作序号'
+        args = arg.split(' ')
+        if(len(args) != 2): 
+            self.ErrorArgs()
+            return
+        self.msg.data = 'manager.' + f'LoadServo("{args[0]}", "Step", {args[1]})'
+        SendCommand(self.msg)
+
+    def do_exec(self, arg):
+        '将传入的参数视为函数调用语句'
+        args = arg.split(' ')
+        self.msg.data = args[0]
+        SendCommand(self.msg)
+
+    def do_vol(self, arg):
+        '获取电量'
+        args = arg.split(' ')
+        if(arg != ''):
+            self.ErrorArgs()
+            return
+        self.msg.data = 'manager.' + f'GetVol()'
+        SendCommand(self.msg)
+
+    def do_exit(self, arg):
+        '退出'
+        print('退出')
+        exit(0)
+
+        
+if __name__ == "__main__":
+    # 日志系统初始化
+    logger = logging.getLogger("Controller")
+    logger.setLevel(logging.INFO)
+    log_file = "/home/yyf/ros03/log/controller.log"
+    try:
+        with open(log_file, 'r') as f:
+            print('打开日志成功')
+    except:
+        with open(log_file, 'w') as f:
+            print("创建新日志")
+    fh = logging.FileHandler(log_file, "a+", encoding="utf-8")
+    fh.setLevel(logging.INFO)
+    formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s")
+    fh.setFormatter(formatter)
+    logger.addHandler(fh)
+
+    #2.初始化 ROS 节点:命名(唯一)
+    rospy.init_node("Controller")
+    #3.实例化 发布者 对象
+    pub = rospy.Publisher("Manager", String, queue_size=1)
+    #4.组织被发布的数据，并编写逻辑发布数据
+
+    # 进入主循环
+    cli = CommandCLI()
+    
+    cli.cmdloop()

hotwheelspkg/scripts/manager.py

+#!/usr/bin/env python
+from cgitb import reset
+import time
+import math
+import sys
+sys.path.append("/home/yyf/ros03/src/hotwheelspkg/scripts")#导入模块，搜索当前目录、已安装内置模块、第三方模块
+from tracemalloc import start
+from uservo import serial, UartServoManager
+from FileManager import *
+import rospy
+from std_msgs.msg import String
+#from calculate import calc_angleup, calc_angledown, calc_posup, calc_posdown
+
+# UART
+SERVO_PORT_NAME =  '/dev/ttyAMA0'		# 舵机串口号
+SERVO_BAUDRATE = 115200			# 舵机的波特率
+# 初始化串口
+uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,
+				parity=serial.PARITY_NONE, stopbits=1,
+				bytesize=8,timeout=0)
+uservo = UartServoManager(uart)
+filefolder = dict()
+
+class ServoManager:
+    # 自定义舵机名称
+    neck = 1            #脖子
+    left_hand = 2       #左上肢
+    left_arm = 3
+    left_shoulder = 4   
+    right_hand = 5      #右上肢
+    right_arm = 6 
+    right_shoulder = 7
+    left_legdown = 8   #左下肢
+    left_legup = 9    
+    left_foot = 10     
+    right_legdown = 11  #右下肢
+    right_legup = 12
+    right_foot = 13
+    left_hip = 14     #臀部
+    right_hip = 15
+    
+    neck = [neck]
+    left_up = [left_hand, left_arm, left_shoulder]
+    right_up = [right_hand, right_arm, right_shoulder] 
+    left_down = [left_legdown, left_legup, left_foot ] 
+    right_down = [right_legdown, right_legup, right_foot]
+    hip = [left_hip, right_hip]
+    ALL = [neck, left_up, right_up, left_down, right_down, hip]
+    servo_num = 15
+    base = []
+   
+    sign = [ +1,                 #neck 
+            +1, -1, +1,        #left_up
+            -1, -1, -1,         #right_up
+            -1, +1, -1,         #left_down
+            +1, +1, +1,         #right_down
+            +1, +1]             #hip
+   
+    #舵机初始化，扫描舵机个数，获得舵机基准状态参数
+    def __init__(self):
+        print("> Manager: 开始初始化舵机...")
+        self.ServoScan()#扫描所有舵机
+        self.LoadBase()#读取base配置文件
+    #展示初始状态动作
+    def Init(self):
+        print("> Manager: 读取舵机RESET配置文件")
+        self.LoadServo('reset', 'Init')
+    #保存初始状态动作
+    def SetInit(self):
+        print("> Manager: 保存初始RESET状态")
+        self.SaveServo('reset', 'Init')
+   
+    #文件操作
+    #显示指定的记录文件内容
+    def ShowFolder(self):
+        print('> FILE: 文件管理器列表: ')
+        for folder in filefolder:
+            print(f'    {folder}/{filefolder[folder].FILE_NAME + filefolder[folder].FILE_EXTENSION}')
+    
+    # 获得单个舵机相对角度
+    def GetSingleAngle(self, servo_index):
+        return self.sign[servo_index - 1] * (int(uservo.query_servo_angle(servo_index)) - self.base[servo_index - 1])
+ 
+    # 获得单个舵机绝对角度
+    def GetSingleAngleAbs(self, servo_index):
+        return int(uservo.query_servo_angle(servo_index))
+    
+    # 获得所有舵机相对角度
+    def GetAllAngle(self):
+        data = []
+        for servo_index in range(1, self.servo_num + 1):
+            data.append(str(self.GetSingleAngle(servo_index)))
+            
+        return ','.join(data) + '\n'
+   
+    # 获得所有舵机绝对角度
+    def GetAllAngleAbs(self):
+        data = []
+        for servo_index in range(1, self.servo_num + 1):
+            data.append(str(self.GetSingleAngleAbs(servo_index)))
+        return ','.join(data) + '\n'
+        print(data)
+  
+    #保存动作，保存相对角度
+    def SaveServo(self, filename, subpath='Defalut', index=0):
+        data = self.GetAllAngle()
+        filefolder[subpath] = FileManager(filename, subpath)
+        filefolder[subpath].WriteFile(data, index)
+   
+    #保存基准状态
+    def SaveBase(self):
+        data = self.GetAllAngleAbs()
+        FileManager('base', 'Init', '.abs').WriteFile(data, 0)
+  
+    #记录时间流
+    def SaveServoFlow(self, filename, recordtime, subpath='TimeFlow'):
+        print('> Manager: 时间流开始记录')
+        self.DampAll()
+        start_time = time.time()
+        lines = []
+        while time.time() - start_time < recordtime:
+            line = []
+            # uservo.query_all_srv_angle()
+            for servo_id in uservo.servos:
+                line.append(str(self.GetSingleAngle(servo_id)))
+            lines.append(line)
+            print("\r> Manager: The program has processed {:2}%".format(100 * (time.time() - start_time) / recordtime), end="")
+        print('')
+        liness = []
+        for line in lines:
+            liness.append(','.join(line) + '\n')
+        filefolder[subpath] = FileManager(filename, subpath, extension='.flow')
+        filefolder[subpath].WriteFileAll(liness)	
+        print('> Maneger: 底层时间流记录完成， 用时', time.time() - start_time)
+  
+    #读取基准状态#
+    def LoadBase(self):
+        print("> Manager: 读取舵机BASE配置文件")
+        filefolder['Init'] = GetFileObject('base', 'Init', '.abs')
+        line = filefolder['Init'].lines[0]
+        data = line[:-1].split(',')
+        for servo_index in range(1, self.servo_num + 1):
+            self.base.append(int(data[servo_index - 1]))
+   
+    #展示动作
+    def LoadServo(self, filename='defalut', subpath='Defalut', index=0, delay=0.0):
+        filefolder[subpath] = GetFileObject(filename, subpath)
+        line = filefolder[subpath].lines[index]
+        data = line[:-1].split(',')
+        for servo_index in range(1, self.servo_num + 1):
+            self.SettingSingleAngle(servo_index, int(data[servo_index - 1]), delay)
+            time.sleep(0.1) #THE LAST STEP
+   
+    #底层时间流运行
+    def LoadServoFlow(self, filename='defalut', subpath='TimeFlow'):
+        print('> Manager: 底层时间流开始运行')
+        start_time = time.time()
+        filefolder[subpath + '/' + filename] = FileManager(filename, subpath, ".flow")
+        lines = filefolder[subpath + '/' + filename].lines
+        for line in lines:
+            step_start_time = time.time()
+            data = line[:-1].split(',')
+            i = 0 
+            for servo_id in uservo.servos:
+                self.SettingSingleAngle(servo_id, int(data[i]))
+                i += 1
+            # time.sleep(5/136 - (time.time() - step_start_time))
+        print('> Manager: 底层时间流运行完成, 用时', time.time() - start_time)
+
+   #发送控制单个舵机角度请求#
+    def SettingSingleAngle(self, servo_index, angle, t=0.0):
+        t = int(1000 * t)
+        angle = self.base[servo_index - 1] + self.sign[servo_index - 1] * angle
+        uservo.set_servo_angle(servo_index, angle, interval=t)
+   
+    #显示所有舵机相对角度#
+    def ShowServoState(self, t=0):
+        showname = ['脖子', '左上肢', '右上肢', '左下肢', '右下肢', '臀部']
+        print("> Manager: 相对角度")
+        for i in range(6):
+                print(f'    {showname[i]}: {[(self.GetSingleAngle(servo_index)) for servo_index in self.ALL[i]]}')
+  
+    #显示所有舵机绝对角度#
+    def ShowServoAbsState(self, t=0):
+        showname = ['脖子', '左上肢', '右上肢', '左下肢', '右下肢', '臀部']
+        print("> Manager: 绝对角度")
+        for i in range(6):
+                print(f'    {showname[i]}: {[(self.GetSingleAngleAbs(servo_index)) for servo_index in self.ALL[i]]}')
+   
+    #扫描舵机，所有的舵机必须首先扫描才可以接受其他指令的控制#
+    def ServoScan(self):
+        global uservo
+        print("> Uservo: 开始进行舵机扫描")
+        uservo.scan_servo()
+        servo_list = list(uservo.servos.keys())
+        print(f"> Uservo: 舵机扫描结束,共{len(servo_list)}个舵机", ", 舵机列表: {}".format(servo_list))
+   
+    #使舵机转变为低阻尼模式，在这种状态下即可手动掰动对应的舵机#
+    def Damp(self, servo_index):#单个
+        uservo.set_damping(servo_index)
+    def DampAll(self):
+        print(uservo.servos)
+        for servo_index in range(1, self.servo_num -7)                                                                                                                        :
+            uservo.set_damping(servo_index)
+            time.sleep(0.1)
+
+
+    #显示电量
+    def GetVol(self):
+        vol_list = [uservo.query_voltage(servo_index) for servo_index in uservo.servos]
+        print('> Manager: Vol: ', sum(vol_list) / len(vol_list))
+  
+    #写入动作，与ws/rs命令类似，但是一个文件只保存一个动作
+    #因此可以通过文件名直接索引动作，不用通过文件名+序号的繁琐方式。
+    #默认子目录为Action
+    def WriteAction(self, act_name):
+        self.ShowServoState(0)
+        self.SaveServo(act_name, 'Action')
+        self.ShowServoState(0)
+
+    #读取动作
+    def ReadAction(self, act_name):
+        self.ShowServoState(0)
+        self.LoadServo(act_name, 'Action')
+        self.ShowServoState(0)
+def response(msg):
+    cmd = msg.data
+    exec(cmd)
+
+#main程序
+if __name__ == "__main__":
+
+    manager = ServoManager()
+
+    msg = String()
+
+    rospy.init_node("UservoSDK")
+
+    sub = rospy.Subscriber("Manager", String, response)
+
+    rate = rospy.Rate(1000)
+    
+    rospy.spin()
+    rate.sleep()

hotwheelspkg/scripts/remote_socket.py

+#!/usr/bin/env python3
+import rospy
+from std_msgs.msg import String
+import socket
+ 
+#2.初始化 ROS 节点:命名(唯一)
+rospy.init_node("Remote_Controller")
+#3.实例化 发布者 对象
+pub = rospy.Publisher("Manager", String, queue_size=1)
+#4.组织被发布的数据，并编写逻辑发布数据
+rate = rospy.Rate(2)
+
+server = socket.socket()
+server.bind(('localhost', 6789))
+server.listen(5)
+ 
+exit = ''
+msg = String()
+while True:
+    con, addr = server.accept()
+    print('connect_addr: ', addr)
+    while con:
+        data = con.recv(1024).decode('utf-8')
+        if data[0:2] == 'ab' and data[-2:] == 'cd':
+            while not rospy.is_shutdown():
+                msg.data = data[2:-2]
+                pub.publish(msg)
+                rate.sleep()
+ 
+            if data == 'break':
+                con.close()
+                exit = 'break'
+                break
+    if exit == 'break':
+        break
+server.close()
\ No newline at end of file

hotwheelspkg/scripts/test.py

+#!/usr/bin/env python
+import pyrealsense2 as rs
+import numpy as np
+import cv2
+import time
+
+if __name__ == "__main__":
+    # Configure depth and color streams
+    import sys
+    print(sys.version)
+    pipeline = rs.pipeline()
+    config = rs.config()
+    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
+    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60)
+    # Start streaming
+    pipeline.start(config)
+    try:
+        while True:
+
+            # Wait for a coherent pair of frames: depth and color
+            frames = pipeline.wait_for_frames()
+            depth_frame = frames.get_depth_frame()
+            color_frame = frames.get_color_frame()
+            if not depth_frame or not color_frame:
+                continue
+            # Convert images to numpy arrays
+
+            depth_image = np.asanyarray(depth_frame.get_data())
+
+            color_image = np.asanyarray(color_frame.get_data())
+
+            # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
+            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
+            # Stack both images horizontally
+            images = np.hstack((color_image, depth_colormap))
+            # Show images
+            cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
+            cv2.imshow('RealSense', images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord('q') or key == 27:
+                cv2.destroyAllWindows()
+                break
+    finally:
+        # Stop streaming
+        pipeline.stop()
+

hotwheelspkg/srv/servo.srv

+uint8 index
+int16 angle
+int16 time
+string cmd
+---
+int16 angle
+string log
+uint8 flag
\ No newline at end of file