def search(start, goal,gridX, gridY, array_of_obst):
AStar.graph=Grid(gridX,gridY)
AStar.graph.obstacles = Grid.find_obstacles(array_of_obst)
AStar.position = start
AStar.found = False
frontier = PriorityQueue()
frontier.put(AStar.position, 0)
came_from = {}
cost_so_far = {}
came_from[AStar.position] = None
cost_so_far[AStar.position] = 0
counter = 0
while not frontier.empty():
current = frontier.get()
#print(current)
next = None
if current == (goal):
AStar.found = True
break
for next in AStar.graph.neighbors(current):
new_cost = cost_so_far[current] + 1
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + AStar.heuristic(goal,next)
frontier.put(next, priority)
came_from[next] = current
if next != None and len(next) > 0:
cv2.circle(Frame.pathplan,Utils.remapPoint(Point(next[0],next[1])).get_coordinate(),5,(0,0,255),2,4)
if Config.obstacleBoundingPointList != None:
Draw.boundingBox(Config.obstacleBoundingPointList)
#show preview of Path finding
Frame.show_frame()
if AStar.found:
print "Found path!!!!!"
path = AStar.FindPath(came_from,goal)
else:
print "Failed to find path!!!!! Bot or Target is inside Obstacle Range!!!! XD"
sleep(10)
return None
AStar.writeToFile(path)
optimizedPathArray = (PathOptimizer.Optimize())
optimizedPathList = []
for node in optimizedPathArray:
#print node
optimizedPathList.append(Utils.remapPoint(Point(int(node[0]),int(node[1]))).get_coordinate())
print "Optimized Path is "
#print optimizedPathList
return optimizedPathList
def findgrid():
Frame.capture_frame()
src = Frame.resized
src = cv2.resize(src,(500,500))
green=(0, 255, 0)
height,width= src.shape[:2]
GRID_SIZE =35
new_y=0
#ector<Rect> mCells
CheckpointType("Resource", "white",(0,255,255))
CheckpointType("Obstacle", "white",(0,255,255))
CheckpointType("Checkpoint", "white",(0,255,255))
CheckpointType("Safe", "white",(0,255,255))
Resource_List = []
Obstacle_List = []
checkPoint_List = []
Safe_List = []
for y in range(0,height,GRID_SIZE): #(y = 0; y < height - GRID_SIZE; y += GRID_SIZE):
for x in range(0,width,GRID_SIZE): #(x = 0; x < width - GRID_SIZE; x += GRID_SIZE):
# k = x*y + x
new_x=x+GRID_SIZE
new_y=y+GRID_SIZE
grid_rect=(x,y),(new_x,new_y)
#print grid_rect
piece=src[y:new_y,x:new_x]
point = Point()
point.x =(x+new_x)/2
point.y =(y+new_y)/2
center=((x+new_x)/2,(y+new_y)/2)
str=a.setarea(piece.copy(),src,center)
if(str=="Resource"):
Resource_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
elif(str=="Obstacle"):
Obstacle_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
elif(str=="Checkpoint"):
checkPoint_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
# if len(checkPoint_List) == 0:
# checkPoint_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
# else:
# for chkpt in checkPoint_List:
# if not Point.chkptinRange(point, chkpt.center):
# checkPoint_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
# cv2.putText(src,"chkpt",center,cv2.FONT_HERSHEY_SIMPLEX,0.4,(0,255,255))
# break
# else:
# print "skipped " + point.toString()
# Safe_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
elif(str=="Safe"):
Safe_List.append(Checkpoint(0, point, 0, 0, CheckpointShape.SQUARE))
# cv2.imshow("PART", piece)
# cv2.waitKey(0)
cv2.rectangle(src, (x,y),(new_x,new_y), green , 1)
#cv2.imshow("src", src)
# cv2.imshow(format("grid%d",k), src(grid_rect))
# cv2.waitKey(0)
cv2.imshow("src", src)
#cv2.waitKey(0)
Frame.src = src
return Resource_List,Obstacle_List,checkPoint_List,Safe_List
import numpy as np
import cv2
import ImageProcess
from ImageProcess import Frame
# blue=
# bgr=cv2.cvtColor(blue,cv2.COLOR_HSV2BGR)
# print bgr
# src = cv2.imread("nitk2.jpg")
Frame.capture_frame()
src = Frame.resized
src = cv2.resize(src,(500,500))
green=(0, 255, 0)
height,width= src.shape[:2]
print src.shape
GRID_SIZE =35
for y in range(0,height,GRID_SIZE): #(y = 0; y < height - GRID_SIZE; y += GRID_SIZE):
for x in range(0,width,GRID_SIZE): #(x = 0; x < width - GRID_SIZE; x += GRID_SIZE):
# k = x*y + x
new_x=x+GRID_SIZE
new_y=y+GRID_SIZE
cv2.rectangle(src, (x,y),(new_x,new_y), green , 1)
cv2.imshow("src", src)
cv2.waitKey(0)
def UpdateProperties():
Config.startTime = int(timeit.default_timer() * 1000)
#assume that you are calling Akshay's Image proccesing function
Frame.capture_frame()
backCheckPointList = Frame.processStream(Bot.botBack)
frontCheckPointList = Frame.processStream(Bot.botFront)
#print str(Frame.isItMyFirstTime)
#print "BOT Contours " + str(len(backCheckPointList)) + " , " + str(len(frontCheckPointList))
if (len(backCheckPointList) <= 0 or len(frontCheckPointList) <= 0):
print "Failed to Capture bot position !!! >>>>>>>>>>>>>> " + str(
len(backCheckPointList)) + " " + str(len(frontCheckPointList))
Bot.moveDirection(Direction.BACKWARD, False)
#sleep(1)
Bot.Stop()
else:
backCheckPoint = None
frontCheckPoint = None
if len(backCheckPointList) > 0 and len(frontCheckPointList) > 0:
Bot.prevBack = backCheckPointList[0]
Bot.prevFront = frontCheckPointList[0]
Bot.position.x = (Bot.prevBack.center.x +
Bot.prevFront.center.x) / 2
Bot.position.y = (Bot.prevBack.center.y +
Bot.prevFront.center.y) / 2
Bot.angle, temp = Utils.angleBetweenPoints(Bot.prevBack.center,
Bot.prevFront.center)
if Bot.runOnce: #Frame.runTimeCounter == 6:
Frame.townHall = Checkpoint(0, copy.deepcopy(Bot.position), 0,
0, 0)
Bot.runOnce = False
Frame.runOnce = False
print("Found bot position")
print Bot.position.toString()
else:
#resource_checkPoints = Frame.processStream(Bot.resource)
#obstacles_checkPoints = Frame.processStream(Bot.obstacle)
#TODO Move to ImageProess
if Config.obstacleBoundingPointList != None:
Draw.boundingBox(Config.obstacleBoundingPointList)
if Bot.currentResource != None:
cv2.circle(Frame.resized,
Bot.currentResource.center.get_coordinate(), 30,
(255, 150, 0), 2, 8)
Draw.circle(Bot.currentResource.path)
#Frame.drawCircle(Bot.currentTarget.center,(255,0,0))
if Bot.currentNode != None:
cv2.circle(Frame.resized, Bot.currentNode.get_coordinate(),
20, (0, 0, 255), 2, 4)
#Frame.drawCircle(Bot.currentNode,(255,0,0))
cv2.putText(
Frame.resized, " Target @" +
Bot.currentTarget.center.toString() + " | A: " +
str(Bot.currentTarget.angle),
Bot.currentTarget.center.get_coordinate(),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 1)
cv2.putText(
Frame.resized, " " +
str(Utils.distance(Bot.position,
Bot.currentTarget.center)),
Utils.midPoint(Bot.position,
Bot.currentTarget.center).get_coordinate(),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
if Bot.currentNode != None:
cv2.arrowedLine(
Frame.resized, Bot.position.get_coordinate(),
Bot.currentNode.get_coordinate(), (255, 150, 0), 2, 0,
0, 0.1
) #draws line from one point ti the other, last arg means thickness
cv2.arrowedLine(
Frame.resized, Bot.prevBack.center.get_coordinate(),
Bot.prevFront.center.get_coordinate(), (255, 255, 255), 10,
0, 0, 1
) #draws line from one point ti the other, last arg means thickness
#draw big arrow on top of BOT
cv2.arrowedLine(
Frame.resized, Bot.prevBack.center.get_coordinate(),
Utils.getPointFromAngle(Bot.prevBack.center,
Bot.prevFront.center),
(255, 255, 25), 1, 0, 0, 1)
Frame.drawCircle(Frame.townHall.center, (0, 255, 255))
if Bot.optimizedAStarPath != None:
Draw.path(Bot.optimizedAStarPath)
#print "Townhall center is:" + str(Frame.townHall.center.toString())
Frame.drawCircle(Bot.position, (0, 255, 0))
cv2.putText(
Frame.resized, " BOT @" +
Bot.position.toString() + " | A: " + str(Bot.angle),
Bot.position.get_coordinate(), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(255, 255, 0), 1)
tempTime = Config.endTime = int(timeit.default_timer() * 1000)
#show time
cv2.putText(
Frame.resized,
"Processing Time : " + str(Config.endTime - Config.startTime),
(10, 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
cv2.putText(Frame.resized,
"Time Elapsed : " + str(Config.endTime / 1000), (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
Frame.show_frame()
Config.endTime = int(timeit.default_timer() * 1000)
#print "Per frame : " + str((Config.endTime - Config.startTime)) + " | Time elapsed : " + str(Config.endTime/1000)
return Bot.position, Bot.angle
def UpdateProperties():
'''
param-None
returns-Bot.position[Type-str], Bot.angle[Type-int]
This function is used loads of times.
If not run before, it initializes the bot's position as the townhall.
Otherwise, it gives back the position of the bot at every instant when it's running.
It also does little bit of image processing in the function botImageProperties() where it shows
the bot process speed and the time elapsed.
But mainly, it gives the bot's position and angle.
'''
Config.startTime = int(timeit.default_timer() * 1000)
Frame.capture_frame()
backCheckPointList = Frame.processStream(Bot.botBack)
frontCheckPointList = Frame.processStream(Bot.botFront)
#print str(Frame.isItMyFirstTime)
#print "BOT Contours " + str(len(backCheckPointList)) + " , " + str(len(frontCheckPointList))
if (len(backCheckPointList) <= 0 or len(frontCheckPointList) <= 0):
print "Failed to Capture bot position !!! >>>>>>>>>>>>>> "
Bot.moveDirection(Direction.BACKWARD, False)
#sleep(1)
Bot.Stop()
else:
backCheckPoint = None
frontCheckPoint = None
if len(backCheckPointList) > 0 and len(frontCheckPointList) > 0:
Bot.prevBack = backCheckPointList[0]
Bot.prevFront = frontCheckPointList[0]
Bot.position.x = (Bot.prevBack.center.x +
Bot.prevFront.center.x) / 2
Bot.position.y = (Bot.prevBack.center.y +
Bot.prevFront.center.y) / 2
Bot.angle, temp = Utils.angleBetweenPoints(Bot.prevBack.center,
Bot.prevFront.center)
if Bot.runOnce: #Frame.runTimeCounter == 6:
Frame.townHall = Checkpoint(0, copy.deepcopy(Bot.position), 0,
0, 0)
Bot.runOnce = False
Frame.runOnce = False
else:
#resource_checkPoints = Frame.processStream(Bot.resource)
#obstacles_checkPoints = Frame.processStream(Bot.obstacle)
Frame.botImageProperties(Bot.currentResource, Bot.currentNode,
Bot.currentTarget, Bot.prevBack,
Bot.prevFront, Bot.position)
if Bot.optimizedAStarPath != None:
Draw.path(Bot.optimizedAStarPath)
#print "Townhall center is:" + str(Frame.townHall.center.toString())
Frame.drawCircle(Bot.position, (0, 255, 0))
'''gives the bot angle'''
cv2.putText(
Frame.resized, " BOT @" +
Bot.position.toString() + " | A: " + str(Bot.angle),
Bot.position.get_coordinate(), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(255, 255, 0), 1)
tempTime = Config.endTime = int(timeit.default_timer() * 1000)
'''show time'''
cv2.putText(
Frame.resized,
"Processing Time : " + str(Config.endTime - Config.startTime),
(10, 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
cv2.putText(Frame.resized,
"Time Elapsed : " + str(Config.endTime / 1000), (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
Frame.show_frame()
Config.endTime = int(timeit.default_timer() * 1000)
#print "Per frame : " + str((Config.endTime - Config.startTime)) + " | Time elapsed : " + str(Config.endTime/1000)
return Bot.position, Bot.angle
from Area import Area
from AStar import *
from BluetoothController import BluetoothController
from BotController import Bot
from Checkpoint import Checkpoint, CheckpointType
from ImageProcess import Frame
from Point import Point
'''connect Bluetooth'''
BluetoothController.connect()
Bot.Stop()
#sleep(1)
Bot.setBotSpeed(200)
#sleep(1)
Frame.connect(1)
Bot.resource = CheckpointType("Resource", "yellow", (0, 255, 255))
Bot.obstacle = CheckpointType("Obstacle", "blue", (255, 0, 0))
Bot.botFront = CheckpointType('botFront', 'green', (0, 255, 0))
Bot.botBack = CheckpointType('botBack', 'red', (0, 0, 255))
raw_input("Start ?????? Press Enter to continue.... : ")
Frame.capture_frame()
Frame.townHall = Checkpoint(0, Point(0, 0), 0, 0, 0)
'''initially find center of townhall by finding bot center'''
Bot.UpdateProperties()
obstacle_checkPoints = Frame.processStream(Bot.obstacle)
Config.obstacleList = obstacle_checkPoints
Config.obstacleCount = len(obstacle_checkPoints)
from BluetoothController import BluetoothController
from BotController import Bot
from Checkpoint import Checkpoint, CheckpointType
from ImageProcess import Frame
from Point import Point
import trig
#
# connect Bluetooth
BluetoothController.connect()
Bot.Stop()
#sleep(1)
Bot.setBotSpeed(50)
sleep(1)
Frame.connect(1)
Bot.resource = CheckpointType("Resource", "white", (0, 255, 255))
Bot.obstacle = CheckpointType("Obstacle", "red", (255, 0, 0))
Bot.botFront = CheckpointType('botFront', 'yellow', (0, 255, 0))
Bot.botBack = CheckpointType('botBack', 'pink', (0, 0, 255))
# Bot.position.x = (Bot.botBack.center.x + Bot.botFront.center.x) / 2
# Bot.position.y = (Bot.botBack.center.y + Bot.botFront.center.y) / 2
# print Bot.position.x,Bot.position.y
# raw_input("Start ?????? Press Enter to continue.... : ")
Frame.capture_frame()
Frame.townHall = Checkpoint(0, Point(315, 315), 0, 0, 0)
#(315,315)