def check_obstacles(self, vehicles):
minimumDistance = 1000
for vehicle in vehicles:
if self.vehicle_type == "Pedrestian": # and vehicle.vehicle_type == "Pedrestian":
continue
elif vehicle.RightOfPassage == 0 and self.RightOfPassage == 1 and self.vehicle_type != "Pedrestian":
# The vehicle doesn't have right of passage (not in roundabout)
continue
elif self.position == vehicle.position:
# It's our vehicle
continue
else:
distance = utils.calc_distance(self.position, vehicle.position)
angle = abs(utils.calc_angle(self.position, vehicle.position) - self.direction)
current_vision = self.vision_angle
if (
self.RightOfPassage == 0
and self.velocity < 0.75 * self.max_velocity
and vehicle.RightOfPassage == 1
):
current_vision = self.vision_angle_entrance
if distance <= self.range_of_sight and angle < current_vision:
stopDistance = (
utils.calc_stopDistance(distance, angle) - self.length * 2 - vehicle.length
) # 3 times radius
if stopDistance < minimumDistance:
minimumDistance = stopDistance
return minimumDistance
def GetDistanceToBusStop(self,position,direction):
deltaX=self.roads[2][0]-self.roads[1][0]
deltaY=self.roads[2][1]-self.roads[1][1]
busStopX=self.roads[1][0]+deltaX*0.5
busStopY=self.roads[1][1]+deltaY*0.5
dist=utils.calc_distance((busStopX,busStopY),position)
if (abs(utils.calc_angle(position, (busStopX,busStopY)) - direction)>math.pi/2):
dist=-dist
return dist
def test_works_as_expected(self):
'''utils.calc_angle correctly calculates the angle between two points'''
input_output_pairs = [[[[0, 0], [1, 0]], 0], [[[1, 0], [0, 0]], np.pi],
[[[0, 0], [1, 1]], np.pi / 4],
[[[0, 0], [0, 1]], np.pi / 2],
[[[1, 1], [0, 0]], 5 * np.pi / 4],
[[[0, 0], [1, 2]],
math.atan(2)], [[[0, 1], [1, 0]],
7 * np.pi / 4]]
for coord_pair, expected_angle in input_output_pairs:
angle = utils.calc_angle(*coord_pair)
self.assertEqual(round(angle, 10), round(expected_angle, 10))
def detect_known_objects(self, img):
print("HELLLOOOOOO")
#img = self.image_resize(img, height=int(img.shape[0]/3.0))
#img_yuv = cv2.cvtColor(img, cv2.COLOR_BGR2YUV)
#img_yuv[:,:,0] = cv2.equalizeHist(img_yuv[:,:,0])
#img=cv2.cvtColor(img_yuv,cv2.COLOR_YUV2BGR)
X, ratio = self.format_img(img, self.C)
if K.image_dim_ordering() == 'tf':
X = np.transpose(X, (0, 2, 3, 1))
# get the feature maps and output from the RPN
[Y1, Y2, F] = self.model_rpn.predict(X)
#print Y1, Y2, F
a = datetime.datetime.now()
R = roi_helpers.rpn_to_roi(Y1,
Y2,
self.C,
K.image_dim_ordering(),
overlap_thresh=0.7)
b = datetime.datetime.now()
delta = b - a
#print("roi_helpers.rpn_to_roi took:", int(delta.total_seconds() * 1000)) # milliseconds
#print R
#for i in R:
# cv2.rectangle(img,(i[0],i[1]),(i[2],i[3]),(0,255,0),3)
# convert from (x1,y1,x2,y2) to (x,y,w,h)
R[:, 2] -= R[:, 0]
R[:, 3] -= R[:, 1]
# apply the spatial pyramid pooling to the proposed regions
bboxes = {}
probs = {}
for idx, jk in enumerate(range(R.shape[0] // self.C.num_rois + 1)):
ROIs = np.expand_dims(R[self.C.num_rois * jk:self.C.num_rois *
(jk + 1), :],
axis=0)
if ROIs.shape[1] == 0:
break
if jk == R.shape[0] // self.C.num_rois:
#pad R
curr_shape = ROIs.shape
target_shape = (curr_shape[0], self.C.num_rois, curr_shape[2])
ROIs_padded = np.zeros(target_shape).astype(ROIs.dtype)
ROIs_padded[:, :curr_shape[1], :] = ROIs
ROIs_padded[0, curr_shape[1]:, :] = ROIs[0, 0, :]
ROIs = ROIs_padded
#print("ROIs shape", np.array(ROIs).shape)
#print("F", np.array(F).shape)
a = datetime.datetime.now()
[P_cls, P_regr,
P_clust] = self.model_classifier_only.predict([F, ROIs])
b = datetime.datetime.now()
delta = b - a
#print("prediction of roi took: :", int(delta.total_seconds() * 1000)) # milliseconds
#print P_cls, P_regr
#print P_cls.shape, P_regr.shape
for ii in range(P_cls.shape[1]):
#print P_cls[0,ii,:]
if np.max(P_cls[0, ii, :]) < self.bbox_threshold or np.argmax(
P_cls[0, ii, :]) == (P_cls.shape[2] - 1):
continue
cls_name = self.class_mapping[np.argmax(P_cls[0, ii, :])]
if cls_name not in bboxes:
bboxes[cls_name] = []
probs[cls_name] = []
(x, y, w, h) = ROIs[0, ii, :]
#print x, y, w, h
cls_num = np.argmax(P_cls[0, ii, :])
#print "something", cls_num
try:
(tx, ty, tw, th) = P_regr[0, ii,
4 * cls_num:4 * (cls_num + 1)]
tx /= self.C.classifier_regr_std[0]
ty /= self.C.classifier_regr_std[1]
tw /= self.C.classifier_regr_std[2]
th /= self.C.classifier_regr_std[3]
x, y, w, h = roi_helpers.apply_regr(
x, y, w, h, tx, ty, tw, th)
except:
print("exception")
pass
bboxes[cls_name].append([
self.C.rpn_stride * x, self.C.rpn_stride * y,
self.C.rpn_stride * (x + w), self.C.rpn_stride * (y + h)
])
probs[cls_name].append(np.max(P_cls[0, ii, :]))
detected_objects = []
for key in bboxes:
bbox = np.array(bboxes[key])
new_boxes, new_probs = roi_helpers.non_max_suppression_fast(
bbox, np.array(probs[key]), overlap_thresh=0.5)
for jk in range(new_boxes.shape[0]):
(x1, y1, x2, y2) = new_boxes[jk, :]
(real_x1, real_y1, real_x2,
real_y2) = self.get_real_coordinates(ratio, x1, y1, x2, y2)
#print "drawing detected rect at:", (real_x1, real_y1), (real_x2, real_y2)
#cv2.rectangle(img,(real_x1, real_y1), (real_x2, real_y2), (int(class_to_color[key][0]), int(class_to_color[key][1]), int(class_to_color[key][2])),5)
#textLabel = '{}: {}'.format(key,int(100*new_probs[jk]))
#all_dets.append((key,100*new_probs[jk]))
#(retval,baseLine) = cv2.getTextSize(textLabel,cv2.FONT_HERSHEY_COMPLEX,1,1)
#textOrg = (real_x1-20, real_y1-20)
#cv2.rectangle(img, (textOrg[0] - 5, textOrg[1]+baseLine - 5), (textOrg[0]+retval[0] +5, textOrg[1]-retval[1] +5), (0, 0, 0), 2)
#cv2.rectangle(img, (textOrg[0] -5,textOrg[1]+baseLine - 5), (textOrg[0]+retval[0] +5, textOrg[1]-retval[1] +5), (255, 255, 255), -1)
#cv2.putText(img, textLabel, textOrg, cv2.FONT_HERSHEY_DUPLEX, 0.3, (0, 0, 0), 1)
height, width, channels = img.shape
#FOV horizontal = 62 degrees (from 90 on right to 33 on left)
angle_between_robot_centre_and_detected_object = self.angle_between(
(real_x1 + self.distance([real_x1, real_y1],
[real_x2, real_y1]) / 2.0,
(real_y1 + self.distance([real_x1, real_y1],
[real_x1, real_y2]) / 2.0)),
(width / 2.0, 0)) - 52.0
angle_between_robot_centre_and_detected_object = -angle_between_robot_centre_and_detected_object
angle_between_robot_centre_and_detected_object = calc_angle([
int((real_x1 + real_x2) / 2.0),
int((real_y1 + real_y2) / 2.0)
])
angle, distance = calculate_angle_and_distance(img,
real_x1,
real_x2,
real_y1,
real_y2,
obj_width=16)
angle_between_robot_centre_and_detected_object = angle
focal_length_mm = 1.0
average_real_object_height_mm = 1.0
image_height_px = height
object_height_px = self.distance([real_x1, real_y1],
[real_x1, real_y2])
sensor_height_mm = 314.2
distance_between_robot_centre_and_detected_object = (15.0 / (
(min(self.distance([real_x1, real_y1], [real_x2, real_y1]),
self.distance([real_x1, real_y1],
[real_x1, real_y2])) /
max(self.distance([real_x1, real_y1], [real_x2, real_y1]),
self.distance([real_x1, real_y1], [real_x1, real_y2]))
)) * 123) / self.distance([real_x1, real_y1],
[real_x2, real_y1])
distance_between_robot_centre_and_detected_object = distance_between_robot_centre_and_detected_object * 5.0
distance_between_robot_centre_and_detected_object = distance
detected_objects.append(
(key, "", real_x1, real_y1, real_x2, real_y2,
distance_between_robot_centre_and_detected_object,
angle_between_robot_centre_and_detected_object))
print("detected objects", len(detected_objects))
temporary_memory = []
for image_item in detected_objects:
seen_item_centroid = (image_item[2] + self.distance(
(image_item[2], image_item[3]),
(image_item[4], image_item[3])) / 2.0,
image_item[4] + self.distance(
(image_item[2], image_item[3]),
(image_item[2], image_item[5])) / 2.0)
tracking_uuid = None
#print ("items in memory", len(self.SHORT_TERM_MEMORY))
for memory_item in self.SHORT_TERM_MEMORY:
memory_centroid = (memory_item[2] + self.distance(
(memory_item[2], memory_item[3]),
(memory_item[4], memory_item[3])) / 2.0,
memory_item[4] + self.distance(
(memory_item[2], memory_item[3]),
(memory_item[2], memory_item[5])) / 2.0)
#print ("distance", self.distance(seen_item_centroid, memory_centroid))
if self.distance(seen_item_centroid,
memory_centroid) < self.distance(
[image_item[2], image_item[3]],
[image_item[4], image_item[5]
]) and image_item[0] == memory_item[0]:
tracking_uuid = memory_item[1]
continue
if tracking_uuid != None:
temporary_memory.append(
(self.KNOWN_OBJECTS[int(image_item[0])], tracking_uuid,
image_item[2], image_item[3], image_item[4],
image_item[5], image_item[6], image_item[7]))
else:
temporary_memory.append(
(self.KNOWN_OBJECTS[int(image_item[0])], uuid.uuid1(),
image_item[2], image_item[3], image_item[4],
image_item[5], image_item[6], image_item[7]))
#print ("temp memory items", len(temporary_memory))
if self.show_image:
for item in temporary_memory:
cv2.rectangle(img, (item[2], item[3]), (item[4], item[5]),
(0, 0, 0), 5)
textLabel = '{}: {}'.format(item[0], item[1])
(retval,
baseLine) = cv2.getTextSize(textLabel,
cv2.FONT_HERSHEY_COMPLEX, 1,
1)
textOrg = (image_item[2] - 20, image_item[3] - 20)
cv2.rectangle(img,
(textOrg[0] - 5, textOrg[1] + baseLine - 5),
(textOrg[0] + retval[0] + 5,
textOrg[1] - retval[1] + 5), (0, 0, 0), 2)
cv2.rectangle(img,
(textOrg[0] - 5, textOrg[1] + baseLine - 5),
(textOrg[0] + retval[0] + 5,
textOrg[1] - retval[1] + 5),
(255, 255, 255), -1)
cv2.putText(img, textLabel, textOrg,
cv2.FONT_HERSHEY_DUPLEX, 0.3, (0, 0, 0), 1)
self.SHORT_TERM_MEMORY = temporary_memory
if self.show_image:
cv2.imshow('image', img)
cv2.waitKey(3000)
#time.sleep(1)
#cv2.destroyAllWindows()
return self.SHORT_TERM_MEMORY
def get_direction(self):
next_pos = self.road.GetNodePosition(self.nextNode)
return utils.calc_angle(self.position, next_pos)
