m = cv.GetImage(cv.fromarray(255 * numpy.ones( (9000,9000,3), numpy.uint8 ) ))
cv.NamedWindow("map")
inner_mask = cv.GetImage(cv.fromarray(numpy.zeros( (600, 600), numpy.uint8 ) ))
outer_mask = cv.GetImage(cv.fromarray(numpy.zeros( (600, 600), numpy.uint8 ) ))
cv.SetImageROI(m, (3700, 3700, 600, 600))
cv.ShowImage("map", m)
toward_white = (60, 60, 60)
toward_black = (120, 120, 120)
try:
listener.daemon = True
listener.start()
while True:
try:
data = q.get(False)
if data:
cv.SetZero(inner_mask)
cv.SetZero(outer_mask)
inner_poly = []
outer_poly = []
cv.ResetImageROI(m)
for data_point in range(len(data)):
i = data_point + start_index
ping = data[data_point] / 20.0
valid_ping = True
if ping == 0:
class Mapper:
def __init__(self):
self.roi_size = 450
self.darken_gradient = (30, 30, 30, 30)
self.lighten_gradient = (70, 70, 70, 70)
self.mm_per_pixel = 15.0
self.lock = False
self.__q = Queue.Queue()
self.__listener = LidarListener(self.__q)
self.__m = cv.GetImage(cv.fromarray(255 * numpy.ones((4000, 4000, 4), numpy.uint8)))
self.__innermask = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
self.__outermask = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
def start(self):
try:
self.__listener.daemon = True
self.__listener.start()
except Exception as ex:
raise ex
def shutdown(self):
try:
self.__listener.kill = True
except Exception as ex:
raise ex
def obstaclePresent(self, abs_center, radius):
roi = (abs_center[0] - radius, abs_center[1] - radius, 2 * radius, 2 * radius)
cv.SetImageROI(self.__m, roi)
result = cv.CreateMat(2 * radius, 2 * radius, cv.CV_8UC4)
cv.AndS(self.__m, (255, 255, 255), result)
result_sum = (255 * 4 * radius * radius) - cv.Sum(result)[0]
"""
conflicts = 0
for i in range(2*radius):
for j in range(2*radius):
if (result[i,j][0] < 255):
conflicts = conflicts + 1
result_sum = conflicts
"""
cv.ResetImageROI(self.__m)
return result_sum > 0.0
# Could be improved, not focused on it right now
def obstaclePresentMask(self, mask, mask_roi):
cv.SetImageROI(self.__m, mask_roi)
result = cv.CreateMat(mask.height, mask.width, cv.CV_8UC4)
cv.AndS(self.__m, (255, 255, 255), result, mask)
conflicts = 0
for i in range(mask.height):
for j in range(mask.width):
if result[i, j][0] < 255 and mask[i, j] != 0:
conflicts = conflicts + 1
# result_sum = ((255*mask.height*mask.width) - cv.Sum(result)[0])
cv.ResetImageROI(self.__m)
return conflicts > 0
def safeLine(self, waypoint, thickness):
cv.ResetImageROI(self.__m)
conflicts = 0
angle = math.radians(cv.FastArctan(waypoint[0][1] - waypoint[1][1], waypoint[0][0] - waypoint[1][0]))
for i in range(
cv.Floor(
cv.Sqrt(math.pow(waypoint[0][0] - waypoint[1][0], 2) + math.pow(waypoint[0][1] - waypoint[1][1], 2))
)
):
check_x = cv.Floor(waypoint[0][0] - i * math.cos(angle))
check_y = cv.Floor(waypoint[0][1] - i * math.sin(angle))
if self.obstaclePresent((check_x, check_y), thickness / 2):
conflicts = conflicts + 1
return conflicts == 0
def mapImage(self):
mapCopy = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
cv.SetImageROI(self.__m, (x - (self.roi_size / 2), y - (self.roi_size / 2), self.roi_size, self.roi_size))
cv.Copy(self.__m, mapCopy)
cv.ResetImageROI(self.__m)
return mapCopy
def mapCopy(self, roi):
mapCopy = cv.GetImage(cv.fromarray(numpy.zeros((roi[2], roi[3]), numpy.uint8)))
cv.SetImageROI(self.__m, roi)
cv.Copy(self.__m, mapCopy)
cv.ResetImageROI(self.__m)
return mapCopy
def mapPointer(self):
return self.__m
def updateFromROS(self, newMap):
self.__m = newMap
cv.ResetImageROI(self.__m)
def scan(self):
x = 2000
y = 2000
heading = 0
try:
data = self.__q.get(False)
if data:
cv.ResetImageROI(self.__m)
cv.SetZero(self.__innermask)
cv.SetZero(self.__outermask)
inner_poly = []
for i in range(len(data)):
ping = data[i] / self.mm_per_pixel
# This needs to be replaced by an error code
valid_ping = True
if ping == 0:
ping = 220
valid_ping = False
# This needs to be replaced with a navdata structure
angle = (i * 0.0062832) + heading - 2.09 + 1.61
ping_x = int(math.floor((self.roi_size / 2) + ping * math.cos(angle)))
ping_y = int(math.floor((self.roi_size / 2) - ping * math.sin(angle)))
# Build the contours of the scanning masks
inner_poly.append((ping_x, ping_y))
if valid_ping == True:
cv.Circle(self.__outermask, (ping_x, ping_y), 3, 255, -1)
# Append the origin to close the polygon's contour
inner_poly.append((x, y))
# Finally create the inner scan mask
cv.FillPoly(self.__innermask, [inner_poly], 255)
# Apply the masks to the map
cv.SetImageROI(
self.__m, (x - (self.roi_size / 2), y - (self.roi_size / 2), self.roi_size, self.roi_size)
)
cv.SubS(self.__m, self.darken_gradient, self.__m, self.__outermask)
cv.AddS(self.__m, self.lighten_gradient, self.__m, self.__innermask)
cv.ResetImageROI(self.__m)
except Exception as ex:
cv.ResetImageROI(self.__m)
pass
