def footPrintCost(self, robot_pose, footprint_points):
#if (footprint_points == None):
# footprint_points = self.mapBaseFootprint
cell = utils.worldToMap(robot_pose, self.getMapOrigin(),
self.getMapResolution())
if (not cell):
return -3.0
line_cost = 0.0
footprint_cost = 0.0
for i in range(len(footprint_points) - 1):
# get first point
p1 = utils.worldToMap(footprint_points[i], self.getMapOrigin(),
self.getMapResolution())
if (p1 == None):
return -3.0
# get last ppoint
pLast = utils.worldToMap(footprint_points[i + 1],
self.getMapOrigin(),
self.getMapResolution())
if (pLast == None):
return -3.0
# form a line and see if any point along line is affected
line_cost = self.lineCost(p1, pLast)
footprint_cost = max(line_cost, footprint_cost)
if (line_cost < 0):
return line_cost
# get first point
p1 = utils.worldToMap(footprint_points[-1], self.getMapOrigin(),
self.getMapResolution())
if (p1 == None):
return -3.0
# get last ppoint
pLast = utils.worldToMap(footprint_points[0], self.getMapOrigin(),
self.getMapResolution())
if (pLast == None):
return -3.0
# form a line and see if any point along line is affected
line_cost = self.lineCost(p1, pLast)
footprint_cost = max(line_cost, footprint_cost)
if (line_cost < 0):
return line_cost
return footprint_cost
def footprintWorldToMap(self, worldFootprint):
mapFootprint = set()
for point in worldFootprint:
print(point)
cell = utils.worldToMap(point, self.getMapOrigin(),
self.getMapResolution())
if cell not in mapFootprint:
mapFootprint.add(cell)
return mapFootprint
def footPrintCostRadius(self, robot_pos):
#print("Robot base: ", self.robotRadiusInCells)
cur_pos = utils.worldToMap(robot_pos, self.getMapOrigin(),
self.getMapResolution())
#print(cur_pos)
grid = self.getBinnedGrid() #dont use raw grid for this
#print(self.mapBaseFootprint)
for x in self.mapBaseFootprint:
temp = [0, 0]
temp[0] = x[0] + cur_pos[0]
temp[1] = x[1] + cur_pos[1]
if (temp[0] >= len(grid) or temp[0] < 0 or temp[1] >= len(grid[0])
or temp[1] < 0):
#print("Invalid point")
return -3.0
if (grid[temp[0]][temp[1]] != 0):
#print("Invalid point")
return -3.0
return 0.0
def getfrontier(mapData, botPos, binned_grid):
print("Getting frontiers")
data = mapData.data
w = mapData.info.width
h = mapData.info.height
#print("Map height: ", h)
#print("Map width: ", w)
resolution = mapData.info.resolution
Xstartx = mapData.info.origin.position.x
Xstarty = mapData.info.origin.position.y
checkOcc = np.uint8(np.array(data).reshape(h, w))
#element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(ERODE_PIXELS,ERODE_PIXELS))
#img4 = cv2.erode(binned_grid,element)
#return binned_grid
#ret,img2 = cv2.threshold(checkOcc,2,255,0)
#This should always be thicker, cuz it' the one being minused off. On the other hand, the canny output cant be too thick cuz that's the one that remains
element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
(DILATE_PIXELS, DILATE_PIXELS))
img4 = cv2.dilate(binned_grid, element)
#return img4 100, 350, 50,350
canny_output = cv2.Canny(
checkOcc, 25, 350
) #cv2.Canny(checkOcc, 225, 250)#cv2.Canny(checkOcc, 100, 150), 125, 350 25,350 #cv2.Canny(checkOcc, 225, 250) #works on gazebo
element = cv2.getStructuringElement(
cv2.MORPH_CROSS, (2, 2)
) #(3,3) maybe too thick so may produce random spots. Thicker may form longer contoues but that isn't priority. Correctness is priority
# (1,1) looks clean but calculating moments inaccurate, will result in [0,0] cuz not enclosed
#(2,2) abit thick but abit no choice ah. Is there better ways to find moments?
# Possibility: (2,2) for moments, (1,1) for final
#Correction: Those white spots appear anyways even with 2,2
thick_canny_output = cv2.dilate(canny_output, element)
# thick_canny_output = edge_output
#return canny_output
#getUnknownEdgesGrid(checkOcc,canny_output,w,h)
edge_output = getUnknownEdgesGrid2(thick_canny_output, img4, w, h)
#to surpress small frontiers?
#https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_morphological_ops/py_morphological_ops.html
# This link kinda prompts the possiblity of making my own kernel
#https://stackoverflow.com/questions/51009126/opencv-how-to-correctly-apply-morphologyex-operation
"""
element = cv2.getStructuringElement(cv2.MORPH_CROSS,(3,3))
edge_output = cv2.dilate(edge_output,element)
element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3,3)) #(2,2)
edge_output = cv2.erode(edge_output,element)
"""
kernel = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
ksize=(2, 2)) #(3,3)
edge_output = cv2.morphologyEx(edge_output, cv2.MORPH_OPEN, kernel)
#return edge_output
contours, hierarchy = cv2.findContours(edge_output, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
# erode abit so that can see the exact frontier? Not exacly working
#element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(2,2)) #MORPH_ELLIPSE,(1,1),MORPH_CROSS
#edge_output = cv2.erode(edge_output,element)
#main_contour = contours[3]
#contoured_image = cv2.drawContours(edge_output, [main_contour], 0, (0,255,0), 1)
contoured_image = cv2.drawContours(
edge_output, contours, -1, (255, 255, 255),
1) #when it outputs 0,0 means failed to find moment?
#return contoured_image
all_pts = []
all_world_pts = []
#print(len(contours))
if len(contours) > 0:
for i in range(0, len(contours)):
cnt = contours[i]
#print("Cnt: ", cnt)
M = cv2.moments(cnt)
cx = int(M['m10'] / (M['m00'] + 1e-5))
cy = int(M['m01'] / (M['m00'] + 1e-5))
if (cx == 0 and cy == 0):
#discard frontier
continue
xr = cx * resolution + Xstartx
yr = cy * resolution + Xstarty
pt = [np.array((cy, cx))]
w_pt = Point()
w_pt.x = xr
w_pt.y = yr
#w_pt = [np.array((xr,yr))]
if len(all_pts) > 0:
all_pts = np.vstack([all_pts, pt])
all_world_pts.append(w_pt)
#all_world_pts = np.vstack([all_world_pts,w_pt])
else:
all_pts = pt
all_world_pts = [w_pt]
#print(all_pts)
res = np.zeros((h, w), dtype=np.uint8)
for points in all_pts:
res[points[0]][points[1]] = 255
#print("World origin: ", Xstartx, Xstarty)
"""
for world_points in all_world_pts:
print(world_points[0], " ", world_points[1])
"""
#print("Bot position in map: ", botPos)
mBotPos = utils.worldToMap(botPos, mapData.info.origin.position,
resolution)
#print("Bot position in cells: ", mBotPos[0], mBotPos[1])
#print("Bot position in map again: ", utils.mapToWorld2(mBotPos, mapData.info.origin.position,resolution))
res[mBotPos[0]][mBotPos[1]] = 255
#just for makring positions
element = cv2.getStructuringElement(cv2.MORPH_CROSS, (7, 7)) #5,5 3,3
res = cv2.dilate(res, element)
contoured_image += res
element = cv2.getStructuringElement(
cv2.MORPH_ELLIPSE,
(5, 5)) #5,8,5MORPH_CROSS, MORPH_ELLIPSE (1,1), (2,2)
img5 = cv2.dilate(binned_grid, element)
return contoured_image, all_pts, all_world_pts, mBotPos, binned_grid, img5 #img4,binned_grid,canny_output,thick_canny_output, img5
def worldToMap(self, point):
return utils.worldToMap(point, self.getMapOrigin(),
self.getMapResolution())
def setRobotPoseCell(self, robot_pose_pos):
self._robot_cell_pose = utils.worldToMap(robot_pose_pos,
self.getMapOrigin(),
self.getMapResolution())