Python CCD读取及计算偏移 from typing import re import serial # 导入串口包 import time # 导入时间包 ser = serial.Serial("COM4", 115200, timeout=5) # 开启com5口，波特率115200，超时5 ser.flushInput() # 清空缓冲区 def main(): #读取CCD数据 msg = ser.read(270).decode("gbk","ignore") a = msg[msg.find("*"):] if len(a) == 270: # print("a:%s" % a) a=a else: a = msg[msg.find("*"):] + ser.read(270).decode()[:270 - len(msg[msg.find("*"):])] # print(msg[msg.find("*"):]) # print("b:" + a) return a def max_length(*lst): return max(*lst, key=lambda v: len(v)) while True: b = main() # print(b) #完整信息 c = b[11:267] # print("16进制:"+c) d=[] n=0 # print(len(a)) while n<255 : c1=c[n:n+2] #两两分割 c2 = int(c[n:n + 2], 16) #进制转化 # print(c) d.append(c2) n=n+2 print("原灰度值列表："+str(d)) #灰度值列表 d_max=str(max(d)) d_min=str(min(d)) # d_min_x=str(d.index(min(d)))#最小值索引 d_ts=str(round(min(d)*1.3)) #调整阈值 print("最大灰度值" + d_max, "最小灰度值：" + d_min, "阈值："+d_ts) # 找出小于阈值的灰度值 list1 = [] temp = [] d_ts_z=[] for i in range(len(d)): if d[i] < round(min(d)*1.3): temp.append(d[i]) else: list1.append(temp) print("阈值以下列表："+str(temp)," 个数："+str(len(temp))) temp1=list(enumerate(d))#原灰度值列表元素元组化 # print("原灰度值列表元组化："+str(temp1)) p=[i for i, j in enumerate(d) if j <min(d)*1.3] #找出列表中小于阈值的灰度值索引 print("阈值以下灰度值坐标："+str(p)) # 方法1：判断坐标连续个数 ==1 # b = [] # for a in range(len(p) - 1): # # print(p[a], p[a] + 1) # # print(p[a-1],p[a]) # if p[a + 1] - p[a] <= 2: # b.append(p[a]) # # print("连续:" + str(a), p[a], p[a + 1], b) # # else: # # print("不连续:" + str(a)) # # print("连续个数为：" + str(a),"连续的坐标为："+str(b)) # 方法2 判断坐标连续个数 s1 = [] # 定义一个空列表 s2 = [] for x in sorted(set(p)): s1.append(x) if x + 1 not in p: if len(s1) > 5:#判断连续多少个 # print(""+str(s1)) s2.append(s1) # else: # print("连续像素未大于6个") s1 = [] print("总"+str(s2)) move = None py_length= None if s2 !=[]: scan=True print("最长列表："+str(max_length(s2))) s3=max_length(s2) if len(s3)%2==0: center = int(len(s3) / 2) center_z=s3[center-1] print("中心坐标为："+str(center_z+0.5),"对应灰度值为："+str(d[center_z])) # print("偶数", center) py_length=round(((center_z+0.5)-64.5)*1) if py_length<0: move="左移" # print(move,py_length) elif py_length==0: move = "中间" # print(move,py_length) else: move="右移" # print(move, str(py_length)+"\n") else: center = int((len(s3) + 1) / 2) center_z=s3[center-1] print("中心坐标为："+str(center_z),"对应灰度值为："+str(d[center_z])) # print("奇数", center) py_length=round(((center_z)-64.5)*1) if py_length<0: move="左移" # print(move,py_length) elif py_length==0: move = "中间" # print(move,py_length) else: move="右移" # print(move, str(py_length)+"\n") else: # print("未识别到\n") scan=False print(scan,move,str(py_length)+"\n") if __name__ == '__main__': main()

python A* 代码 import math import matplotlib.pyplot as plt from numpy import arange show_animation = True class AStarPlanner: def __init__(self, ox, oy, resolution, rr): """ Initialize grid map for a star planning ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] resolution: grid resolution [m] rr: robot radius[m] """ self.resolution = resolution self.rr = rr self.min_x, self.min_y = 0, 0 self.max_x, self.max_y = 0, 0 self.obstacle_map = None self.x_width, self.y_width = 0, 0 self.motion = self.get_motion_model() self.calc_obstacle_map(ox, oy) class Node: def __init__(self, x, y, cost, parent_index): self.x = x # index of grid self.y = y # index of grid self.cost = cost self.parent_index = parent_index def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( self.cost) + "," + str(self.parent_index) def planning(self, sx, sy, gx, gy): """ A star path search input: s_x: start x position [m] s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] output: rx: x position list of the final path ry: y position list of the final path """ start_node = self.Node(self.calc_xy_index(sx, self.min_x), self.calc_xy_index(sy, self.min_y), 0.0, -1) goal_node = self.Node(self.calc_xy_index(gx, self.min_x), self.calc_xy_index(gy, self.min_y), 0.0, -1) open_set, closed_set = dict(), dict() open_set[self.calc_grid_index(start_node)] = start_node while 1: if len(open_set) == 0: print("Open set is empty..") break c_id = min( open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[ o])) current = open_set[c_id] # show graph if show_animation: # pragma: no cover plt.plot(self.calc_grid_position(current.x, self.min_x), self.calc_grid_position(current.y, self.min_y), "xc") # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit( 0) if event.key == 'escape' else None]) if len(closed_set.keys()) % 10 == 0: plt.pause(0.001) if current.x == goal_node.x and current.y == goal_node.y: print("Find goal") goal_node.parent_index = current.parent_index goal_node.cost = current.cost break # Remove the item from the open set del open_set[c_id] # Add it to the closed set closed_set[c_id] = current # expand_grid search grid based on motion model for i, _ in enumerate(self.motion): node = self.Node(current.x + self.motion[i][0], current.y + self.motion[i][1], current.cost + self.motion[i][2], c_id) n_id = self.calc_grid_index(node) # If the node is not safe, do nothing if not self.verify_node(node): continue if n_id in closed_set: continue if n_id not in open_set: open_set[n_id] = node # discovered a new node else: if open_set[n_id].cost > node.cost: # This path is the best until now. record it open_set[n_id] = node rx, ry = self.calc_final_path(goal_node, closed_set) return rx, ry def calc_final_path(self, goal_node, closed_set): # generate final course rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ self.calc_grid_position(goal_node.y, self.min_y)] parent_index = goal_node.parent_index while parent_index != -1: n = closed_set[parent_index] rx.append(self.calc_grid_position(n.x, self.min_x)) ry.append(self.calc_grid_position(n.y, self.min_y)) parent_index = n.parent_index return rx, ry @staticmethod def calc_heuristic(n1, n2): w = 1.0 # weight of heuristic d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) return d def calc_grid_position(self, index, min_position): """ calc grid position :param index: :param min_position: :return: """ pos = index * self.resolution + min_position return pos def calc_xy_index(self, position, min_pos): return round((position - min_pos) / self.resolution) def calc_grid_index(self, node): return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) def verify_node(self, node): px = self.calc_grid_position(node.x, self.min_x) py = self.calc_grid_position(node.y, self.min_y) if px < self.min_x: return False elif py < self.min_y: return False elif px >= self.max_x: return False elif py >= self.max_y: return False # collision check if self.obstacle_map[node.x][node.y]: return False return True def calc_obstacle_map(self, ox, oy): self.min_x = round(min(ox)) self.min_y = round(min(oy)) self.max_x = round(max(ox)) self.max_y = round(max(oy)) print("min_x:", self.min_x) print("min_y:", self.min_y) print("max_x:", self.max_x) print("max_y:", self.max_y) self.x_width = round((self.max_x - self.min_x) / self.resolution) self.y_width = round((self.max_y - self.min_y) / self.resolution) print("x_width:", self.x_width) print("y_width:", self.y_width) # obstacle map generation self.obstacle_map = [[False for _ in range(self.y_width)] for _ in range(self.x_width)] for ix in range(self.x_width): x = self.calc_grid_position(ix, self.min_x) for iy in range(self.y_width): y = self.calc_grid_position(iy, self.min_y) for iox, ioy in zip(ox, oy): d = math.hypot(iox - x, ioy - y) if d <= self.rr: self.obstacle_map[ix][iy] = True break @staticmethod def get_motion_model(): # dx, dy, cost motion = [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1], [-1, -1, math.sqrt(2)], [-1, 1, math.sqrt(2)], [1, -1, math.sqrt(2)], [1, 1, math.sqrt(2)]] return motion def main(): print(__file__ + " start!!") # start and goal position #起点 sx = 10.0 # [dm] sy = 10.0 # [dm] #终点 gx = 112.0 # [dm] gy = 65.0 # [dm] grid_size = 2.0 # [dm] robot_radius = 1.5 # [dm] 机器人半径 # set obstacle positions ox, oy = [], [] for i in arange(0,136.79): #下 ox.append(i) oy.append(0.0) for i in arange(0, 72.70): #右 ox.append(136.79) oy.append(i) for i in arange(0, 137): #上 ox.append(i) oy.append(72.70) for i in arange(0, 72.70): #左 ox.append(0.0) oy.append(i) # 讲台桌子 for i in arange(25.43, 45.43):#左 ox.append(13.43) oy.append(i) for i in arange(25.43, 46):#右 ox.append(20.43) oy.append(i) for i in arange(13.43, 20.43):#下 ox.append(i) oy.append(25.43) for i in arange(13.43, 20.43):#上 ox.append(i) oy.append(45.43) # 1桌子(左下) for i in arange(13.43, 19.37):#左 ox.append(19.45) oy.append(i) for i in arange(13.43, 20):#右 ox.append(79.45) oy.append(i) for i in arange(19.45, 79.45):#下 ox.append(i) oy.append(13.43) for i in arange(19.37, 79.45):#上 ox.append(i) oy.append(19.45) # 2桌子（右下） for i in arange(13.43, 19.37):#左 ox.append(84.38) oy.append(i) for i in arange(13.43, 20):#右 ox.append(132.39) oy.append(i) for i in arange(84.38, 132.39):#下 ox.append(i) oy.append(13.43) for i in arange(84.38, 132.39):#上 ox.append(i) oy.append(19.37) # 3桌子（左上） for i in arange(53.72, 59.72):#左 ox.append(19.45) oy.append(i) for i in arange(53.72, 60):#右 ox.append(79.45) oy.append(i) for i in arange(19.45, 79.45):#下 ox.append(i) oy.append(53.72) for i in arange(19.45, 79.45):#上 ox.append(i) oy.append(59.72) # 4桌（右上） for i in arange(53.72, 59.72):#左 ox.append(84.38) oy.append(i) for i in arange(53.72, 60):#右 ox.append(132.39) oy.append(i) for i in arange(84.38, 132.39):#下 ox.append(i) oy.append(53) for i in arange(84.38, 132.39):#上 ox.append(i) oy.append(59.72) # 下桌 for i in arange(19.37, 31.38):#左 ox.append(119.95) oy.append(i) for i in arange(19.37, 31.38):#右 ox.append(125.95) oy.append(i) for i in arange(119.95, 125.95):#上 ox.append(i) oy.append(31.38) # 上桌 for i in arange(41.72, 53.72):#左 ox.append(119.95) oy.append(i) for i in arange(41.72, 53.72):#右 ox.append(125.95) oy.append(i) for i in arange(119.95, 125.95):#下 ox.append(i) oy.append(41.72) # 下空调 for i in arange(0, 4.47):#左 ox.append(55.53) oy.append(i) for i in arange(0, 4.47):#右 ox.append(60.58) oy.append(i) for i in arange(55.53, 60.58):#上 ox.append(i) oy.append(4.47) # 上空调 for i in arange(68.23, 72.70):#左 ox.append(57.49) oy.append(i) for i in arange(68.23, 72.70):#右 ox.append(62.54) oy.append(i) for i in arange(57.49, 62.54):#下 ox.append(i) oy.append(68.23) # 上空调 for i in arange(66.26, 72.70):#左 ox.append(118.22) oy.append(i) for i in arange(118.22, 136.79):#下 ox.append(i) oy.append(66.26) if show_animation: # pragma: no cover plt.plot(ox, oy, ".k") plt.plot(sx, sy, "og") plt.plot(gx, gy, "xb") plt.grid(True) plt.axis("equal") a_star = AStarPlanner(ox, oy, grid_size, robot_radius) rx, ry = a_star.planning(sx, sy, gx, gy) if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.pause(0.001) plt.title('6604 A*') plt.show() if __name__ == '__main__': main()pic

AGV读码器udp数据接收(待完善) {dotted startColor="#ff6c6c" endColor="#1989fa"/}单次import socket #1.创建UDP套接字 #2.绑定地址（host,port）到套接字 s.bind() #3.收发数据 s.recvfrom() s.sendto() #4.关闭服务器端套接字 s.close() s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) host = "" port = 7788 s.bind((host,port)) #绑定地址（host,port）到套接字 #接收客户端传过来的数据 data,addr = s.recvfrom(1024) #data是接收到的数据 addr是对方的地址 也就是发送方的地址 a=str(data) #print(type(data)) c=a.split("<")[-1] d=c.split(">")[0] find="@(" if find not in a: print("未识别到二维码") else:print("收到的数据为：",d) #print("addr",addr) s.close()循环import socket #1.创建UDP套接字 #2.绑定地址（host,port）到套接字 s.bind() #3.收发数据 s.recvfrom() s.sendto() #4.关闭服务器端套接字 s.close() s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) host = "" port = 7788 s.bind((host,port)) #绑定地址（host,port）到套接字 while True: #接收客户端传过来的数据 data,addr = s.recvfrom(1024) #data是接收到的数据 addr是对方的地址 也就是发送方的地址 a=str(data) c = a.split("<")[-1] d = a.split(">")[0] find = "@(" if find not in a: print("未识别到二维码") #print(type(data)) else:print("收到的数据为：",c) #print("addr",addr) s.close()

python A*(地图导出mysql+excel) run.pyimport xlsxwriter as xlsxwriter from astar import astar from Map import Map import pandas as pd import sqlalchemy mymap = [ [1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1], [1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1] ] map = Map(mymap,0,1,6,25) result = astar(map) result.reverse() print(result) num=len(result) m='' for i in range(num): x=str(result[i]) m=m+x workbook = xlsxwriter.Workbook('C:/Users/17709/Desktop/map.xlsx') worksheet = workbook.add_worksheet() # Widen the first column to make the text clearer # 设置一列或者多列单元属性 worksheet.set_column('A:A', 30) # Add a bold format to use to highlight cells # 在工作表中创建一个新的格式对象来格式化单元格，实现加粗 bold = workbook.add_format({'bold': True}) # Write some numbers, with row/column notation #按照坐标写入 worksheet.write(0,0,'') worksheet.write(1,0,'1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(2,0,'1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(3,0,'1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(4,0,'1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(5,0,'1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(6,0,'1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(7,0,'1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(8,0,'1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(9,0,'1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1') worksheet.write(12,0,m) # 关闭工作薄 workbook.close() print('已导出至Excel') engine=sqlalchemy.create_engine('mysql+pymysql://用户名:密码@ip:3306/map') #读取数据 db=pd.read_excel('C:/Users/17709/Desktop/map.xlsx') #导入数据库 db.to_sql(name='1',con=engine,index=False,if_exists='replace') print('已导入数据库')astar.pyfrom Node import Node def getKeyforSort(element:Node): return element.g #element#不应该+element.h，否则会穿墙 def astar(workMap): startx,starty = workMap.startx,workMap.starty endx,endy = workMap.endx,workMap.endy startNode = Node(startx, starty, 0, 0, None) openList = [] lockList = [] lockList.append(startNode) currNode = startNode while((endx,endy) != (currNode.x,currNode.y)): workList = currNode.getNeighbor(workMap.data,endx,endy) for i in workList: if (i not in lockList): if(i.hasNode(openList)): i.changeG(openList) else: openList.append(i) openList.sort(key=getKeyforSort)#关键步骤 currNode = openList.pop(0) lockList.append(currNode) result = [] while(currNode.father!=None): result.append((currNode.x,currNode.y)) currNode = currNode.father result.append((currNode.x,currNode.y)) return resultNode.py''' Node.py主要是描述对象Node ''' class Node(object): ''' 初始化节点信息 ''' def __init__(self,x,y,g,h,father): self.x = x self.y = y self.g = g self.h = h self.father = father ''' 处理边界和障碍点 ''' def getNeighbor(self,mapdata,endx,endy): x = self.x y = self.y result = [] #先判断是否在上下边界 #if(x!=0 or x!=len(mapdata)-1): #上 #Node(x,y,g,h,father) if(x!=0 and mapdata[x-1][y]!=0): upNode = Node(x-1,y,self.g+10,(abs(x-1-endx)+abs(y-endy))*10,self) result.append(upNode) #下 if(x!=len(mapdata)-1 and mapdata[x+1][y]!=0): downNode = Node(x+1,y,self.g+10,(abs(x+1-endx)+abs(y-endy))*10,self) result.append(downNode) #左 if(y!=0 and mapdata[x][y-1]!=0): leftNode = Node(x,y-1,self.g+10,(abs(x-endx)+abs(y-1-endy))*10,self) result.append(leftNode) #右 if(y!=len(mapdata[0])-1 and mapdata[x][y+1]!=0): rightNode = Node(x,y+1,self.g+10,(abs(x-endx)+abs(y+1-endy))*10,self) result.append(rightNode) #西北 14 if(x!=0 and y!=0 and mapdata[x-1][y-1]!=0 ): wnNode = Node(x-1,y-1,self.g+14,(abs(x-1-endx)+abs(y-1-endy))*10,self) result.append(wnNode) #东北 if(x!=0 and y!=len(mapdata[0])-1 and mapdata[x-1][y+1]!=0 ): enNode = Node(x-1,y+1,self.g+14,(abs(x-1-endx)+abs(y+1-endy))*10,self) result.append(enNode) #西南 if(x!=len(mapdata)-1 and y!=0 and mapdata[x+1][y-1]!=0 ): wsNode = Node(x+1,y-1,self.g+14,(abs(x+1-endx)+abs(y-1-endy))*10,self) result.append(wsNode) #东南 if(x!=len(mapdata)-1 and y!=len(mapdata[0])-1 and mapdata[x+1][y+1]!=0 ): esNode = Node(x+1,y+1,self.g+14,(abs(x+1-endx)+abs(y+1-endy))*10,self) result.append(esNode) # #如果节点在关闭节点 则不进行处理 # finaResult = [] # for i in result: # if(i not in lockList): # finaResult.append(i) # result = finaResult return result def hasNode(self,worklist): for i in worklist: if(i.x==self.x and i.y ==self.y): return True return False #在存在的前提下 def changeG(self,worklist): for i in worklist: if(i.x==self.x and i.y ==self.y): if(i.g>self.g): i.g = self.gMap.pyimport math ''' 对象Map，主要有地图数据、起点和终点 ''' class Map(object): def __init__(self,mapdata,startx,starty,endx,endy): self.data = mapdata self.startx = startx self.starty = starty self.endx = endx self.endy = endy