def __init__(self, f, canvas): self.fig = f self.canvas = canvas self.pathTrackOn = True; self.height = 400 self.width = 600 self.x0 = 0 self.y0 = 0 self.x1 = self.x0 + self.width self.y1 = self.y0 + self.height self.time = 0.0 self.TIMEINC = 0.1 self.robotDOF = ThreeDOF.ThreeDOF() self.robotDOF.xy.x = 1 self.robotDOF.xy.y = 1 self.utility = SimulatorUtility(self) self.inputMove = ThreeDOF.ThreeDOF() self.obstacles = self.utility.initObstacles(self) self.measureDists = [] self.measureHits = [] self.goal = Coordinate.Coordinate(400, 250) self.map = Map(self) self.astar = Astar(self, self.map) path = self.astar.calculate(self.robotDOF.xy, self.goal) self.astarPathX = [] self.astarPathY = [] # print('path = ', path) for p in path: self.astarPathX.append(p.x0) self.astarPathY.append(p.y0) self.pathCount = 0
class Simulator(threading.Thread): def __init__(self, f, canvas): self.fig = f self.canvas = canvas self.pathTrackOn = True; self.height = 400 self.width = 600 self.x0 = 0 self.y0 = 0 self.x1 = self.x0 + self.width self.y1 = self.y0 + self.height self.time = 0.0 self.TIMEINC = 0.1 self.robotDOF = ThreeDOF.ThreeDOF() self.robotDOF.xy.x = 1 self.robotDOF.xy.y = 1 self.utility = SimulatorUtility(self) self.inputMove = ThreeDOF.ThreeDOF() self.obstacles = self.utility.initObstacles(self) self.measureDists = [] self.measureHits = [] self.goal = Coordinate.Coordinate(400, 250) self.map = Map(self) self.astar = Astar(self, self.map) path = self.astar.calculate(self.robotDOF.xy, self.goal) self.astarPathX = [] self.astarPathY = [] # print('path = ', path) for p in path: self.astarPathX.append(p.x0) self.astarPathY.append(p.y0) self.pathCount = 0 def run(self): print('\n\n') print('robot xy = ', self.robotDOF.xy.x, self.robotDOF.xy.y, self.robotDOF.theta) self.time += self.TIMEINC moveStep = 7 # self.inputMove.xy.x = random.uniform(0,moveStep) # self.inputMove.xy.y = random.uniform(0,moveStep) self.inputMove.xy.x = self.astarPathX[self.pathCount] - self.robotDOF.xy.x self.inputMove.xy.y = self.astarPathY[self.pathCount] - self.robotDOF.xy.y self.pathCount += 1 self.moveTo(self.inputMove) SimulatorMeasure.measure(self) # print(self.measureDists) self.utility.drawSimulator(self) self.canvas.draw() self.canvas.flush_events() # self.canvas.show() return True def TurnOnOffPathTrack(self, withPathTrack): if (withPathTrack): self.pathTrackOn = True else: self.pathTrackOn = False def moveTo(self, inputMoveDOF): lineMovement = LineSegment.LineSegment(self.robotDOF.xy.x, self.robotDOF.xy.y, self.robotDOF.xy.x+inputMoveDOF.xy.x, self.robotDOF.xy.y+inputMoveDOF.xy.y) if Simulator.checkMovement(self.obstacles, lineMovement): # if True: self.robotDOF.xy.x = lineMovement.ex self.robotDOF.xy.y = lineMovement.ey self.robotDOF.xy.theta = self.robotDOF.theta + inputMoveDOF.theta return True else: return False # inpute relative translation @staticmethod def checkMovement(obstacles, lineMovement): for obs in obstacles: ret = obs.checkLineSegCross(lineMovement) # print(ret) if ret is not None: # print(ret) return False else: return True