def get_test_bounding_boxes():
Helipad = BoundingBox()
Helipad.probability = 0.5
Helipad.xmin = 312
Helipad.ymin = 120
Helipad.xmax = 337
Helipad.ymax = 148
Helipad.id = 2
Helipad.Class = "Helipad"
H = BoundingBox()
H.probability = 0.5
H.xmin = 320
H.ymin = 128
H.xmax = 330
H.ymax = 138
H.id = 0
H.Class = "H"
Arrow = BoundingBox()
Arrow.probability = 0.5
Arrow.xmin = 333
Arrow.ymin = 140
Arrow.xmax = 335
Arrow.ymax = 143
Arrow.id = 1
Arrow.Class = "Arrow"
bbs = BoundingBoxes()
bbs.bounding_boxes = [Helipad, H, Arrow]
return bbs
def msgDN(self, image, boxes, scores, classes):
"""
Create the Object Detector message to publish with ROS
This uses the Darknet BoundingBox[es] messages
"""
msg = BoundingBoxes()
msg.header = image.header
scores_above_threshold = np.where(scores > self.threshold)[1]
for s in scores_above_threshold:
# Get the properties
bb = boxes[0, s, :]
sc = scores[0, s]
cl = classes[0, s]
# Create the bounding box message
detection = BoundingBox()
detection.Class = self.category_index[int(cl)]['name']
detection.probability = sc
detection.xmin = int((image.width - 1) * bb[1])
detection.ymin = int((image.height - 1) * bb[0])
detection.xmax = int((image.width - 1) * bb[3])
detection.ymax = int((image.height - 1) * bb[2])
msg.boundingBoxes.append(detection)
return msg
def process(msg):
t_begin = rospy.get_time()
cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
cv2.imwrite(img_path, cv_img)
with open(img_path, "rb") as img:
img_bytes = img.read()
server.send(darksocket.pack(darksocket.Packet.IMAGE, img_bytes))
res = server.recv()
t_end = rospy.get_time()
if telemetry:
cls()
rospy.loginfo("%s FPS" % (1 / (t_end - t_begin)))
if res == darksocket.StreamEvent.DETECTIONS:
dets = Detections()
dets.image = msg
for det in server.stream.detections:
bbox = BoundingBox()
bbox.Class = det[0]
bbox.probability = det[1]
bbox.xmin = det[2]
bbox.ymin = det[3]
bbox.xmax = det[2] + det[4]
bbox.ymax = det[3] + det[5]
dets.bboxes.append(bbox)
if telemetry:
rospy.loginfo("%s %s" % (bbox.Class, bbox.probability))
pub_detections.publish(dets)
def callback(self, data):
print('\n', 'Predicting')
# try:
# cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
# except CvBridgeError as e:
# print(e)
# cv_image = cv2.copyMakeBorder(cv_image, 100, 100, 100, 100, cv2.BORDER_CONSTANT, value=[85, 120, 68])
# cv_image = cv2.imread("pic_buffer/R.png")
# cv2.imwrite("pic_buffer/R.png", cv_image)
# im = array_to_image(cv_image)
# dn.rgbgr_image(im)
# # print(dn.detect(net, meta, im))
# print("===")
r = []
img = cv2.imread("pic_buffer/1_R.png")
sp = img.shape
r += self.subpredict(0, int(sp[0] / 2), 0, sp[1])
r += self.subpredict(int(sp[0] / 2), sp[0], 0, sp[1])
r += self.subpredict(int(sp[0] / 4), int(sp[0] * 3 / 4), 0, sp[1])
r += self.subpredict(0, sp[0], 0, int(sp[1] / 2))
r += self.subpredict(0, sp[0], int(sp[1] / 2), sp[1])
r += self.subpredict(0, sp[0], int(sp[1] / 3), int(sp[1] * 2 / 3))
r += self.subpredict(0, sp[0], 0, int(sp[1] * 2 / 3))
r += self.subpredict(0, sp[0], int(sp[1] / 3), sp[1])
r += self.subpredict(0, int(sp[0] * 2 / 3), 0, sp[1])
r += self.subpredict(int(sp[0] / 3), sp[0], 0, sp[1])
r = self.removeDuplicate(r)
print()
img = self.drawCrossings(img, r)
cv2.imwrite("pic_buffer/2_D.png", img)
boxes = BoundingBoxes()
print('\n', 'Predict result:')
for i in range(len(r)):
box = BoundingBox()
print(' ', r[i][0], r[i][1] * 100, '%')
box.Class = r[i][0].decode('utf-8')
box.probability = r[i][1]
box.id = i
box.xmin = int(r[i][2][0])
box.ymin = int(r[i][2][1])
box.xmax = int(r[i][2][2])
box.ymax = int(r[i][2][3])
boxes.bounding_boxes.append(box)
# if b'endpoint' == r[i][0]:
# print('\t', r[i][0], r[i][1]*100, '%')
print(' ', int(r[i][2][0]), int(r[i][2][1]), int(r[i][2][2]),
int(r[i][2][3]))
# if b'cross' == r[i][0]:
# print('\t', r[i][0], r[i][1]*100, '%')
# # print('\t', r[i][2])
print('\n', 'Darknet waiting for rgb_img')
time.sleep(0.5)
try:
self.boxes_pub.publish(boxes)
except CvBridgeError as e:
print(e)
def camera_image_callback(self, image, camera):
"""Gets images from camera to generate detections on
Computes where the bounding boxes should be in the image, and
fakes YOLO bounding boxes output as well as publishing a debug
image showing where the detections are if turned on
Parameters
----------
image : sensor_msgs.Image
The image from the camera to create detections for
camera : Camera
Holds camera parameters for projecting points
"""
cv_image = self.bridge.imgmsg_to_cv2(image, desired_encoding="rgb8")
if camera.is_ready():
# Fake darknet yolo detections message
bounding_boxes = BoundingBoxes()
bounding_boxes.header = image.header
bounding_boxes.image_header = image.header
bounding_boxes.image = image
bounding_boxes.bounding_boxes = []
for _, obj in self.objects.iteritems():
detections = self.get_detections(obj, camera, image.header.stamp)
if detections is not None:
if camera.debug_image_pub:
self.render_debug_context(cv_image, detections, camera)
for det in detections:
bounding_box = BoundingBox()
bounding_box.Class = det[2]
bounding_box.probability = 1.0
bounding_box.xmin = int(det[0][0])
bounding_box.xmax = int(det[0][1])
bounding_box.ymin = int(det[0][2])
bounding_box.ymax = int(det[0][3])
bounding_boxes.bounding_boxes.append(bounding_box)
# Only publish detection if there are boxes in it
if bounding_boxes.bounding_boxes:
self.darknet_detection_pub.publish(bounding_boxes)
if camera.debug_image_pub:
image_message = self.bridge.cv2_to_imgmsg(cv_image, encoding="rgb8")
camera.debug_image_pub.publish(image_message)
def image_callback(self, msg):
# print("Received an image!")
try:
# Convert your ROS Image message to numpy image data type
cv2_img = np.frombuffer(msg.data, dtype=np.uint8).reshape(
msg.height, msg.width, -1)
image_ori = cv2.resize(cv2_img, (512, 512))
#remove alpha channel if it exists
if image_ori.shape[-1] == 4:
image_ori = image_ori[..., :3]
#normalize the image
image = self.normalize(image_ori)
with torch.no_grad():
image = torch.Tensor(image)
if torch.cuda.is_available():
image = image.cuda()
boxes = self.model_.get_boxes(
image.permute(2, 0, 1).unsqueeze(0))
Boxes_msg = BoundingBoxes()
Boxes_msg.image_header = msg.header
Boxes_msg.header.stamp = rospy.Time.now()
for box in boxes[0]:
# print(box.confidence)
confidence = float(box.confidence)
box = (box.box * torch.Tensor([512] * 4)).int().tolist()
if confidence > 0.35:
cv2.rectangle(image_ori, (box[0], box[1]),
(box[2], box[3]), (255, 0, 0), 2)
cv2.putText(image_ori,
str(confidence)[:4], (box[0] - 2, box[1] - 2),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
# msg_frame = self.bridge.cv2_to_imgmsg(image_ori)
detection_box = BoundingBox()
# detection_box.Class=str(box.class_id)
detection_box.xmin = box[0]
detection_box.ymin = box[1]
detection_box.xmax = box[2]
detection_box.ymax = box[3]
detection_box.probability = confidence
Boxes_msg.bounding_boxes.append(detection_box)
msg_frame = self.bridge.cv2_to_imgmsg(image_ori)
self.img_pub.publish(msg_frame)
self.ret_pub.publish(Boxes_msg)
except CvBridgeError:
print("error")
if self.savefigure:
cv2.imwrite('new_image.jpeg', cv2_img)
print('save picture')
# self.detected_msg.data="Take a photo"
self.savefigure = False
def create_d_msgs_box(self, track):
one_box = BoundingBox()
one_box.id = int(track.id[:3], 16)
one_box.class_id = "face"
one_box.probability = float(track.score)
one_box.xmin = int(track.box[0])
one_box.ymin = int(track.box[1])
one_box.xmax = int(track.box[2])
one_box.ymax = int(track.box[3])
return one_box
def test1():
g = GateLocalizer()
image_dims = (480,752)
# image is 480x752
# does x go along the rows or columns??
boxes = []
boxLeft = BoundingBox()
boxLeft.xmin = 607
boxLeft.ymin = 154
boxLeft.xmax = 629
boxLeft.ymax = 410
boxRight = BoundingBox()
boxRight.xmin = 206
boxRight.ymin = 171
boxRight.xmax = 231
boxRight.ymax = 391
boxes.append(boxLeft)
boxes.append(boxRight)
print(g.midpoint(boxes, image_dims))
def _write_message(self, detection_results, boxes, scores, classes):
""" populate output message with input header and bounding boxes information """
if boxes is None:
return None
for box, score, category in zip(boxes, scores, classes):
# Populate darknet message
left, bottom, right, top = box
detection_msg = BoundingBox()
detection_msg.xmin = left
detection_msg.xmax = right
detection_msg.ymin = top
detection_msg.ymax = bottom
detection_msg.probability = score
detection_msg.Class = category
detection_results.bounding_boxes.append(detection_msg)
return detection_results
def GetBBoxesMsg(self, label_file: str):
output_bboxes_msg = BoundingBoxes()
with open(label_file, 'r') as file:
id_ = 0
for line in file:
gt = line.split()
bbox_msg = BoundingBox()
#print(gt)
bbox_msg.xmin = int(gt[5])
bbox_msg.ymin = int(gt[6])
bbox_msg.xmax = int(gt[7])
bbox_msg.ymax = int(gt[8])
bbox_msg.id = id_
bbox_msg.Class = gt[0]
id_ += 1
output_bboxes_msg.bounding_boxes.append(bbox_msg)
return output_bboxes_msg
def msg(image, boxes):
"""
Create the Darknet BoundingBox[es] messages
"""
msg = BoundingBoxes()
msg.header = image.header
for (x, y, w, h) in boxes:
detection = BoundingBox()
detection.Class = "human"
detection.probability = 1 # Not really...
detection.xmin = x
detection.ymin = y
detection.xmax = x + w
detection.ymax = y + h
msg.boundingBoxes.append(detection)
return msg
def test_tracker_callback(self):
args = None
rclpy.init(args=args)
node = ObjectTrackingNode()
bboxes_msg = BoundingBoxes()
for track in self.tracks:
bbox_msg = BoundingBox()
bbox_msg.xmin = track["x1"]
bbox_msg.ymin = track["y1"]
bbox_msg.xmax = track["x2"]
bbox_msg.ymax = track["y2"]
bboxes_msg.bounding_boxes.append(bbox_msg)
node.tracker_callback(bboxes_msg)
def boxcallback(self, msg):
dets = []
for i in range(len(msg.bounding_boxes)):
bbox = msg.bounding_boxes[i]
dets.append(
np.array([
bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax,
bbox.probability
]))
dets = np.array(dets)
start_time = time.time()
trackers = self.update(dets)
cycle_time = time.time() - start_time
print(cycle_time)
r = BoundingBoxes()
rb = BoundingBox()
for d in range(len(trackers)):
rb.probability = 1
rb.xmin = trackers[d, 0]
rb.ymin = trackers[d, 1]
rb.xmax = trackers[d, 2]
rb.ymax = trackers[d, 3]
rb.id = trackers[d, 4]
rb.Class = 'tracked'
r.bounding_boxes.append(rb)
if self.img_in == 1 and self.display:
res = trackers[d].astype(np.int32)
rgb = self.colours[res[4] % 32, :] * 255
cv2.rectangle(self.img, (res[0], res[1]), (res[2], res[3]),
(rgb[0], rgb[1], rgb[2]), 2)
cv2.putText(self.img, "ID : %d" % (res[4]), (res[0], res[1]),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (200, 85, 200), 2)
if self.img_in == 1 and self.display:
try:
self.image = self.bridge.cv2_to_imgmsg(self.img, "bgr8")
self.image.header.stamp = rospy.Time.now()
self.pubimage.publish(self.image)
except CvBridgeError as e:
pass
r.header.stamp = rospy.Time.now()
self.pubb.publish(r)
return
while True:
try:
start_time = time.time()
if mot_tracker.bbox_checkin==1:
trackers = mot_tracker.update(mot_tracker.dets)
mot_tracker.bbox_checkin=0
else:
trackers = mot_tracker.update(np.empty((0,5)))
r = BoundingBoxes()
for d in range(len(trackers)):
rb = BoundingBox()
rb.probability=1
rb.xmin = int(trackers[d,0])
rb.ymin = int(trackers[d,1])
rb.xmax = int(trackers[d,2])
rb.ymax = int(trackers[d,3])
rb.id = int(trackers[d,4])
rb.Class = 'tracked'
r.bounding_boxes.append(rb)
if mot_tracker.img_in==1 and mot_tracker.display:
res = trackers[d].astype(np.int32)
rgb=colours[res[4]%32,:]*255
cv2.rectangle(mot_tracker.img, (res[0],res[1]), (res[2],res[3]), (rgb[0],rgb[1],rgb[2]), 6)
cv2.putText(mot_tracker.img, "ID : %d"%(res[4]), (res[0],res[1]), cv2.FONT_HERSHEY_SIMPLEX, 1.6, (200,85,200), 6)
if mot_tracker.img_in==1 and mot_tracker.display:
try :
mot_tracker.image = mot_tracker.bridge.cv2_to_imgmsg(mot_tracker.img, "bgr8")
mot_tracker.image.header.stamp = rospy.Time.now()
mot_tracker.pubimage.publish(mot_tracker.image)
while True:
try:
start_time = time.time()
if mot_tracker.bbox_checkin == 1:
trackers = mot_tracker.update(mot_tracker.dets)
mot_tracker.bbox_checkin = 0
else:
trackers = mot_tracker.update(np.empty((0, 5)))
r = BoundingBoxes()
for d in range(len(trackers)):
rb = BoundingBox()
rb.probability = 1
rb.xmin = trackers[d, 0]
rb.ymin = trackers[d, 1]
rb.xmax = trackers[d, 2]
rb.ymax = trackers[d, 3]
rb.id = trackers[d, 4]
rb.Class = 'tracked'
r.bounding_boxes.append(rb)
if mot_tracker.img_in == 1 and mot_tracker.display:
res = trackers[d].astype(np.int32)
rgb = colours[res[4] % 32, :] * 255
cv2.rectangle(mot_tracker.img, (res[0], res[1]),
(res[2], res[3]), (rgb[0], rgb[1], rgb[2]),
6)
cv2.putText(mot_tracker.img, "ID : %d" % (res[4]),
(res[0], res[1]), cv2.FONT_HERSHEY_SIMPLEX,
1.6, (200, 85, 200), 6)
if mot_tracker.img_in == 1 and mot_tracker.display: