Example #1
0
            #  dstar2.dstar(mapper, robot, goal, path)

            dstar.dstar(mapper, robot, goal, path)

        #dlite.dstar(robot,goal,path)
        #  #  DstarLite.dstar(mapper, robot, goal, path)
        #  astar.astar(mapper, robot, goal, path)
        pathNode = path.getNextMove()
        robot.x = pathNode.x
        robot.y = pathNode.y
        mapper.moveGrid[pathNode.x][pathNode.y] = "1"
        #mapper.printMoveGrid()
        TEG.x = pathNode.x
        TEG.y = pathNode.y
        TEG.addPosition(pathNode.x, pathNode.y)
        mapper.updateMap(robot)
        #raw_input("TEST")
        cv2.imshow('AGVC Map', agvcmap.getMap())
        cv2.imshow('AGVC Map Morphed', agvcmap.getImage())
        for i in range(len(TEG.positions)):
            agvcmap.placeRobot(TEG.positions[i][1] * height / newHeight,
                               TEG.positions[i][0] * width / newWidth,
                               TEG.radius)
        nPoints = len(TEG.positions)
        points = np.array(TEG.positions) * height / newHeight
        #points = np.random.rand(nPoints,2)*200
        xpoints = [p[0] for p in points]
        ypoints = [p[1] for p in points]

        xvals, yvals = bezier_curve(points, nTimes=1000)
        for i in range(len(xvals)):
Example #2
0
            #  dstar2.dstar(mapper, robot, goal, path)

            dstar.dstar(mapper, robot, goal, path)
     
        #dlite.dstar(robot,goal,path)
         #  #  DstarLite.dstar(mapper, robot, goal, path)
          #  astar.astar(mapper, robot, goal, path)
        pathNode=path.getNextMove()
        robot.x = pathNode.x
        robot.y = pathNode.y
        mapper.moveGrid[pathNode.x][pathNode.y]="1"
        #mapper.printMoveGrid()
        TEG.x = pathNode.x
        TEG.y = pathNode.y
        TEG.addPosition(pathNode.x,pathNode.y)
        mapper.updateMap(robot)
        #raw_input("TEST")
        cv2.imshow('AGVC Map', agvcmap.getMap())
        cv2.imshow('AGVC Map Morphed', agvcmap.getImage())
        for i in range(len(TEG.positions)):
            agvcmap.placeRobot(TEG.positions[i][1]*height/newHeight,TEG.positions[i][0]*width/newWidth,TEG.radius)
        nPoints = len(TEG.positions)
        points = np.array(TEG.positions)*height/newHeight
        #points = np.random.rand(nPoints,2)*200
        xpoints = [p[0] for p in points]
        ypoints = [p[1] for p in points]

        xvals, yvals = bezier_curve(points, nTimes=1000)
        for i in range(len(xvals)):
            agvcmap.placeRobot(int(yvals[i]),int(xvals[i]),TEG.radius*2)
Example #3
0
	def calculatePath(self):
		height = self.map.height
		width = self.map.width
		newHeight = int(height*0.1)
		newWidth = int(width*0.1)
		dliteimage = cv2.resize(self.map.getImage(),(newWidth,newHeight))
		cv2.imwrite('AGVCmap2.bmp', dliteimage)
		robot = Robot(self.vehicle.x,self.vehicle.y,self.vehicle.radius*2)
		imageMap = ImageReader()
		imageMap.loadFile("AGVCmap2.bmp")
		mapper.initalize(imageMap,robot)
		moveGrid = imageMap.convertToGrid().copy()

		##goal = point(3,17)
		testdivider = 1
		self.goal = point(int(newHeight/testdivider*0.8),int(newWidth/testdivider*0.8))
		#cv2.waitKey(0)

		##mapper.printMoveGrid()

		print "STARTIN LOOP"
		moveId=0
		Xlength = mapper.grid.shape[0]/testdivider
		Ylength = mapper.grid.shape[1]/testdivider
		#dstar = dstar3.DStar(Xlength,Ylength,goal)
		dstar = dlite.Dlite(Xlength,Ylength,self.goal,robot)
		print "Entering Loop"
		testvar = 0

		#for i in range(10):
		while (robot.y != self.goal.y or robot.x != self.goal.x) :
			if testvar%2 == 0:
				newObs = obstacle.Obstacle(random.randint(0,height),random.randint(0,width), 40)
				self.map.placeObstacle(newObs,3)
				self.obsList.append(newObs)
				#Place obstacles on map
				self.map.updateObstacles(self.obsList)

				#Morph the obstacles
				self.map.updateMorph();
				dliteimage = cv2.resize(self.map.getImage(),(newWidth,newHeight))
				cv2.imwrite('AGVCmap2.bmp', dliteimage)
				imageMap.loadFile("AGVCmap2.bmp")
				mapper.initalize(imageMap,robot)
				moveGrid = imageMap.convertToGrid().copy()
				
			testvar = testvar + 1

			moveId = moveId+1
			print moveId
			if path.pathIsBroken(mapper.grid) :
				path.restart()
				print "The path is broken"
				#  dstar2.dstar(mapper, robot, goal, path)

				dstar.dstar(mapper, robot, self.goal, path)
		 
			#dlite.dstar(robot,goal,path)
			 #  #  DstarLite.dstar(mapper, robot, goal, path)
			  #  astar.astar(mapper, robot, goal, path)
			pathNode=path.getNextMove()
			robot.x = pathNode.x
			robot.y = pathNode.y
			mapper.moveGrid[pathNode.x][pathNode.y]="1"
			#mapper.printMoveGrid()
			self.vehicle.x = pathNode.x
			self.vehicle.y = pathNode.y
			self.vehicle.addPosition(pathNode.x,pathNode.y)
			mapper.updateMap(robot)
			#raw_input("TEST")
			cv2.imshow('AGVC Map', self.map.getMap())
			cv2.imshow('AGVC Map Morphed', self.map.getImage())
			for i in range(len(self.vehicle.positions)):
				self.map.placeRobot(self.vehicle.positions[i][1]*height/newHeight,self.vehicle.positions[i][0]*width/newWidth,self.vehicle.radius)
			nPoints = len(self.vehicle.positions)
			points = np.array(self.vehicle.positions)*height/newHeight
			#points = np.random.rand(nPoints,2)*200
			xpoints = [p[0] for p in points]
			ypoints = [p[1] for p in points]

			xvals, yvals = self.bezier_curve(points, nTimes=1000)
			for i in range(len(xvals)):
				self.map.placeRobot(int(yvals[i]),int(xvals[i]),self.vehicle.radius*2)

			self.map.updateActObstacles(self.obsList)
        
			cv2.waitKey(0)
Example #4
0
    def calculatePath(self):
        height = self.map.height
        width = self.map.width
        newHeight = int(height * 0.1)
        newWidth = int(width * 0.1)
        dliteimage = cv2.resize(self.map.getImage(), (newWidth, newHeight))
        cv2.imwrite('AGVCmap2.bmp', dliteimage)
        robot = Robot(self.vehicle.x, self.vehicle.y, self.vehicle.radius * 2)
        imageMap = ImageReader()
        imageMap.loadFile("AGVCmap2.bmp")
        mapper.initalize(imageMap, robot)
        moveGrid = imageMap.convertToGrid().copy()

        ##goal = point(3,17)
        testdivider = 1
        self.goal = point(int(newHeight / testdivider * 0.8),
                          int(newWidth / testdivider * 0.8))
        #cv2.waitKey(0)

        ##mapper.printMoveGrid()

        print "STARTIN LOOP"
        moveId = 0
        Xlength = mapper.grid.shape[0] / testdivider
        Ylength = mapper.grid.shape[1] / testdivider
        #dstar = dstar3.DStar(Xlength,Ylength,goal)
        dstar = dlite.Dlite(Xlength, Ylength, self.goal, robot)
        print "Entering Loop"
        testvar = 0

        #for i in range(10):
        while (robot.y != self.goal.y or robot.x != self.goal.x):
            if testvar % 2 == 0:
                newObs = obstacle.Obstacle(random.randint(0, height),
                                           random.randint(0, width), 40)
                self.map.placeObstacle(newObs, 3)
                self.obsList.append(newObs)
                #Place obstacles on map
                self.map.updateObstacles(self.obsList)

                #Morph the obstacles
                self.map.updateMorph()
                dliteimage = cv2.resize(self.map.getImage(),
                                        (newWidth, newHeight))
                cv2.imwrite('AGVCmap2.bmp', dliteimage)
                imageMap.loadFile("AGVCmap2.bmp")
                mapper.initalize(imageMap, robot)
                moveGrid = imageMap.convertToGrid().copy()

            testvar = testvar + 1

            moveId = moveId + 1
            print moveId
            if path.pathIsBroken(mapper.grid):
                path.restart()
                print "The path is broken"
                #  dstar2.dstar(mapper, robot, goal, path)

                dstar.dstar(mapper, robot, self.goal, path)

            #dlite.dstar(robot,goal,path)
            #  #  DstarLite.dstar(mapper, robot, goal, path)
            #  astar.astar(mapper, robot, goal, path)
            pathNode = path.getNextMove()
            robot.x = pathNode.x
            robot.y = pathNode.y
            mapper.moveGrid[pathNode.x][pathNode.y] = "1"
            #mapper.printMoveGrid()
            self.vehicle.x = pathNode.x
            self.vehicle.y = pathNode.y
            self.vehicle.addPosition(pathNode.x, pathNode.y)
            mapper.updateMap(robot)
            #raw_input("TEST")
            cv2.imshow('AGVC Map', self.map.getMap())
            cv2.imshow('AGVC Map Morphed', self.map.getImage())
            for i in range(len(self.vehicle.positions)):
                self.map.placeRobot(
                    self.vehicle.positions[i][1] * height / newHeight,
                    self.vehicle.positions[i][0] * width / newWidth,
                    self.vehicle.radius)
            nPoints = len(self.vehicle.positions)
            points = np.array(self.vehicle.positions) * height / newHeight
            #points = np.random.rand(nPoints,2)*200
            xpoints = [p[0] for p in points]
            ypoints = [p[1] for p in points]

            xvals, yvals = self.bezier_curve(points, nTimes=1000)
            for i in range(len(xvals)):
                self.map.placeRobot(int(yvals[i]), int(xvals[i]),
                                    self.vehicle.radius * 2)

            self.map.updateActObstacles(self.obsList)

            cv2.waitKey(0)