def visualize(self):
if not self.do_viz:
return
if self.nearest_pt_pub.get_num_connections()>0 and isinstance(self.nearest_pt, np.ndarray):
self.nearest_pt_pub.publish(utils.make_circle_marker(self.nearest_pt, 0.5, [0, 0, 1], "/map", "/charging_navi", 0, 3))
if self.lookahead_pt_pub.get_num_connections()>0 and isinstance(self.lookahead_pt, np.ndarray):
self.lookahead_pt_pub.publish(utils.make_circle_marker(self.lookahead_pt, 0.5, [1, 1, 1], "/map", "/charging_navi", 1, 3))
def visualize(self, current_pose):
''' Publishes visualization topics:
- Circle to indicate the nearest point along the trajectory
- Circle to indicate the chosen lookahead point '''
if not self.do_viz:
return
# visualize: pure pursuit circle, lookahead intersection, lookahead radius line, nearest point
if self.nearest_pt_pub.get_num_connections() > 0 and isinstance(
self.nearest_point, np.ndarray):
self.nearest_pt_pub.publish(
utils.make_circle_marker(self.nearest_point, 0.5,
[0.0, 0.0, 1.0], "/map",
"/pure_pursuit", 0, 3))
if self.lookahead_pt_pub.get_num_connections() > 0 and isinstance(
self.lookahead_point, np.ndarray):
self.lookahead_pt_pub.publish(
utils.make_circle_marker(self.lookahead_point, 0.5,
[1.0, 1.0, 1.0], "/map",
"/pure_pursuit", 1, 3))
if self.lookahead_pt_pub.get_num_connections() > 0 and isinstance(
self.lookahead_point, np.ndarray) and isinstance(
current_pose, np.ndarray):
self.lookahead_dist_pub.publish(
utils.make_line_marker(self.lookahead_point, current_pose, 0.3,
[1.0, 1.0, 0.0], "/map",
"/pure_pursuit", 0, 3))
def main(self):
i = 0
while True:
for location in self.objects:
angle = location[1]
index = max([
min([
int(self.width - (math.tan(angle) * self.constant +
self.width / 2)), self.width
]), 0
])
if index <= 200:
left = 0
right = 400
elif index >= self.width - 400:
left = self.width - 401
right = self.width - 1
else:
left = index - 200
right = index + 200
if index < self.width / 2:
#cv2.imshow("FRAME",self.image_hsv_L[int(self.height*0.3):int(self.height*0.9),left:right])
#cv2.waitKey(1)
hsv = self.image_hsv_L[int(self.height *
0.45):int(self.height * 0.9),
left:right]
else:
#cv2.imshow("FRAME",self.image_hsv_R[int(self.height*0.3):int(self.height*0.9),left:right])
#cv2.waitKey(1)
hsv = self.image_hsv_R[int(self.height *
0.45):int(self.height * 0.9),
left:right]
maskOrange = cv2.inRange(
hsv, self.coneOrangeLower,
self.coneOrangeUpper) #check for cone HSV range
maskOrange = cv2.erode(maskOrange, None,
iterations=1) #remove noise
maskGreen = cv2.inRange(
hsv, self.coneGreenLower,
self.coneGreenUpper) #check for cone HSV range
maskGreen = cv2.erode(maskGreen, None,
iterations=1) #remove noise
#cv2.imshow("mask",maskOrange)
#cv2.waitKey(1)
#cv2.imshow("mask",maskGreen)
#cv2.waitKey(1)
cntsOrange = cv2.findContours(maskOrange.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
cntsGreen = cv2.findContours(maskGreen.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
area_O = 0
if len(cntsOrange) > 0:
c = max(cntsOrange, key=cv2.contourArea)
area_O = cv2.contourArea(c)
area_G = 0
if len(cntsGreen) > 0:
c = max(cntsGreen, key=cv2.contourArea)
area_G = cv2.contourArea(c)
if area_O > area_G:
color = "orange"
else:
color = "green"
if max([area_O, area_G]) > 400:
map_location = self.relative_to_map(
location[0], location[1])
self.permissible_region = copy.deepcopy(
self.O_permissible_region)
cv2.circle(self.permissible_region, map_location, 20, 0,
-1)
#cv2.imshow('frame', self.permissible_region)
#cv2.waitKey(1) # just want to see what it looks like
#print location
#print color
world = self.relative_to_world(location[0], location[1])
cone_msg = cone()
cone_msg.distance = location[0]
cone_msg.angle = location[1]
cone_msg.exists = False
cone_msg.color = color
self.pub.publish(cone_msg)
if True: #if visualize
if color == "orange":
c = [1.0, 0.0, 0.0]
else:
c = [0.0, 1.0, 0.0]
self.cone_viz_pub.publish(
Utils.make_circle_marker(world, 0.5, c, "/map",
"cone_viz", 0, 60))
break