import json import time import threading import base import rest from config import cfg from actuator import Actuator, ONE_MOVE_DURATION, ONE_TURN_DURATION from sonic import Measurer import astar ONE_MOVE_INTERVAL_TIME = cfg.getfloat("space", "onemovetime") #s, 移动1个坐标距离的时长。注意：acturator的ONE_MOVE_DURATION要比该值大，确保运行不卡顿 OBSTACLE_AVAILABLE_DISTANCE = cfg.getint("space", "obstacledistance") #mm。检测到障碍物的有效距离 OBSTACLE_ASSUME_FORBID_WIDTH = cfg.getint("space", "obstacleassumewidth") #dm。假设障碍物是35cm，robot是35cm，障碍物屏蔽掉的宽度不仅是障碍物本身，还有左右各半个robot的宽度（以确保robot可穿行，类似于地图边线要离实际边线大于半个robot宽度） OBSTACLE_WAIT_TIME = cfg.getfloat("space", "obstaclewaittime") #s OBSTACLE_MEASURE_TIME = cfg.getfloat("space", "obstaclemeasuretime") RelayActURL = base.joinURL(cfg.get("relay","addr"), "relay/set") class _findway(): def __init__(self): self.currentpos = base.Position(base.Point(0, 0), 0) #当前所在位置。angle=0, x轴正向/东向；angle=90，y轴正向/北向；...(笛卡尔坐标系) self.mapborders = [] self.hasmap = self.LoadMap() self.astar = astar.AStar(self.mapborders) def LoadMap(self): mapdir = cfg.get("space","mapdir") with open(mapdir, "r") as f: mapdict = json.load(f) p = mapdict["currentpos"] self.currentpos.point.x = p["x"] self.currentpos.point.y = p["y"] self.currentpos.angle = p["angle"] mappointers = mapdict["mappointers"] count = len(mappointers) for i in range(0, count) : curpoint = mappointers[i] if i < count -1 : nextpoint = mappointers[i+1] else: nextpoint = mappointers[0] self.mapborders.append(base.Border(curpoint["x"], curpoint["y"], nextpoint["x"], nextpoint["y"], curpoint["through"], nextpoint["through"])) return True def Find(self, endpoint, haveobstacle): # 如果有障碍物，计算障碍物坐标 obstaclestartpoint = base.Point(0,0) # obstacle对应笛卡尔坐标系中的左下角 obstaclewidth = OBSTACLE_ASSUME_FORBID_WIDTH if haveobstacle : if self.currentpos.angle == 0: obstaclestartpoint.x = self.currentpos.point.x + 1 obstaclestartpoint.y = self.currentpos.point.y - round(OBSTACLE_ASSUME_FORBID_WIDTH/2) elif self.currentpos.angle == 180: obstaclestartpoint.x = self.currentpos.point.x - OBSTACLE_ASSUME_FORBID_WIDTH obstaclestartpoint.y = self.currentpos.point.y - round(OBSTACLE_ASSUME_FORBID_WIDTH/2) elif self.currentpos.angle == 90: obstaclestartpoint.x = self.currentpos.point.x - round(OBSTACLE_ASSUME_FORBID_WIDTH/2) obstaclestartpoint.y = self.currentpos.point.y + 1 else: obstaclestartpoint.x = self.currentpos.point.x -round(OBSTACLE_ASSUME_FORBID_WIDTH/2) obstaclestartpoint.y = self.currentpos.point.y - OBSTACLE_ASSUME_FORBID_WIDTH # 采用simple astar算法寻路 waypositions = self.astar.FindWay(self.currentpos.point, endpoint, haveobstacle, obstaclestartpoint, obstaclewidth) return waypositions def GetRobotPosInfo(self): global _amthreadsinglelock if _amthreadsinglelock : return {"x": self.currentpos.point.x, "y": self.currentpos.point.y, "angle": self.currentpos.angle, "status": 1} else: return {"x": self.currentpos.point.x, "y": self.currentpos.point.y, "angle": self.currentpos.angle, "status": 0} def GetMapPoints(self): points = [] for b in self.mapborders: point = {"x": b.start.x, "y": b.start.y} points.append(point) return points _findwayobj = _findway() _amthreadsinglelock = False class _automovethread(threading.Thread): def __init__(self, token, endpoint): threading.Thread.__init__(self) self.token = token self.endpoint = endpoint # point of the end position self.running = False def run(self): if not _findwayobj.hasmap : return global _amthreadsinglelock if _amthreadsinglelock: return _amthreadsinglelock = True if Measurer.Open(self.token) : if Measurer.StartMeasures(self.token, OBSTACLE_MEASURE_TIME) : self.running = True haveobstacle = False while self.running: noerror = True waypositions = _findwayobj.Find(self.endpoint, haveobstacle) if waypositions is None: # 如果找不到路： if haveobstacle : # 在有障碍的情况下找不到路，说明障碍堵严了去路，等待障碍自己移动开，再重新计算路径，否则持续循环 self._obstacleblock() #持续阻塞 waypositions = _findwayobj.Find(self.endpoint, False) else : # 在无障碍的情况下，说明移动失败，直接跳出循环 break haveobstacle = False for pos in waypositions: if not self.running: break self.__turnbyangle(pos.angle) #1. 调整到面向正确的角度 errcode = self.__walkbyline(pos.point) #2.按照路径点进行线性行走 if errcode == -1 : #3.如果返回路径错误，则重新规划路径点集，并重新开始循环 noerror = False break elif errcode == -2 : #4.如果返回障碍阻拦，则重新规划路径点集，并重新开始循环 haveobstacle = True noerror = False break elif errcode == -3 : #6. 如果人为终止walkbyline，则表示终止循环，跳出 break if noerror: #如果没有发生错误，表示顺利完成移动，跳出循环。如果有错误，则继续while循环，以当前位置为起点重新规划路线。 self.running = False Measurer.StopMeasures(self.token) Measurer.Close(self.token) self.running = False _amthreadsinglelock = False def IsRunning(self): return _amthreadsinglelock def StopThread(self): self.running = False while(self.is_alive()): time.sleep(1) return True def __turnbyangle(self, angle): offset = angle - _findwayobj.currentpos.angle if offset == 0 : return elif offset == 90 or offset == -270 : self.__left() elif offset == -90 or offset == 270 : self.__right() elif offset == 180 or offset == -180: self.__back() def __walkbyline(self, endpoint): predistance = _findwayobj.currentpos.point.Distance(endpoint) while self.running: currdistance = _findwayobj.currentpos.point.Distance(endpoint) if currdistance == 0: #1.判断是否已经到达目的点 self.__stop() return 0 if currdistance > predistance: #2.判断是否远离目的点，如已经远离目标点，返回路径错误 self.__stop() return -1 predistance = currdistance if self.__haveobstacle() : #3.如果有障碍 Actuator.Stop(self.token) time.sleep(OBSTACLE_WAIT_TIME) #4.等待一段时间，看障碍是否会离开 if self.__haveobstacle() : #5.如障碍仍存在，返回障碍阻拦错误 return -2 self.__forward() #6.如果没有障碍，或者障碍已经离开，则直行 return -3 def __haveobstacle(self): d = Measurer.GetMeasuresDistance(self.token) # print("measure: %d" % d) return d < OBSTACLE_AVAILABLE_DISTANCE def _obstacleblock(self): while self.__haveobstacle() and self.running : time.sleep(OBSTACLE_WAIT_TIME) def __forward(self): #直行一个步长 Actuator.Forward(self.token) time.sleep(ONE_MOVE_INTERVAL_TIME) #为了保持持续前进，需小于ONE_MOVE_DURATION，且恰好前进一个步长 _findwayobj.currentpos.Forward() def __left(self): Actuator.Left(self.token) time.sleep(ONE_TURN_DURATION) _findwayobj.currentpos.Left() def __right(self): Actuator.Right(self.token) time.sleep(ONE_TURN_DURATION) _findwayobj.currentpos.Right() def __back(self): Actuator.Back(self.token) time.sleep(ONE_TURN_DURATION*2) _findwayobj.currentpos.Back() def __stop(self): Actuator.Stop(self.token) class _space: def __init__(self): self.automovethread = None def StartAutoMove(self, token, endpoint): if self.automovethread is not None: if self.automovethread.IsRunning() : return False self.automovethread = _automovethread(token, endpoint) self.automovethread.start() return True def StopAutoMove(self): if self.automovethread is not None: if self.automovethread.IsRunning(): self.automovethread.StopThread() return True def GetRobotPosInfo(self): return _findwayobj.GetRobotPosInfo() def GetMapPoints(self): return _findwayobj.GetMapPoints() Space = _space()