Ejemplo n.º 1
0
    #    temp = cv2.dilate(eroded,element)
    #    temp = cv2.subtract(img,temp)
    #    skel = cv2.bitwise_or(skel,temp)
    #    img = eroded.copy()

    #    zeros = size - cv2.countNonZero(img)
    #    if zeros==size:
    #        done = True

    #cv2.imshow("skel",skel)
    #D* Lite Method ---------------------------------------------
    newHeight = int(height * 0.1)
    newWidth = int(width * 0.1)
    dliteimage = cv2.resize(agvcmap.getImage(), (newWidth, newHeight))
    cv2.imwrite('AGVCmap2.bmp', dliteimage)
    robot = Robot(TEG.x, TEG.y, TEG.radius * 2)
    imageMap = ImageReader()
    imageMap.loadFile("AGVCmap2.bmp")
    mapper.initalize(imageMap, robot)
    moveGrid = imageMap.convertToGrid().copy()

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

    ##mapper.printMoveGrid()

    print "STARTIN LOOP"
    moveId = 0
Ejemplo n.º 2
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)