def _get_test_messages(self,
max_y,
max_x,
n_grid=2,
box_height=200,
box_width=200):
"""Internal use only. Returns a list of fake detections
Args:
max_y (int): Max y position (height) of the bounding boxes
max_x (int): Max x position (width) of the bounding boxes
"""
detections = []
margin = 20
max_y -= box_height + (margin * 2)
max_x -= box_width + (margin * 2)
for i in range(n_grid + 1):
y = max_y * (i / n_grid) + margin
for j in range(n_grid + 1):
x = max_x * (j / n_grid) + margin
roi = RegionOfInterest(x1=x,
x2=x + box_width,
y1=y,
y2=y + box_height)
seg_roi = SegmentOfInterest(x=range(int(roi.x1), int(roi.x2)),
y=range(int(roi.y1), int(roi.y2)))
detections.append(
Detection(info=DetectionInfo(score=np.random.uniform(),
class_id=-1,
class_name="test"),
roi=roi,
seg_roi=seg_roi))
return detections
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections = Detections()
try:
image = ros_numpy.numpify(request.image)
if request.image.encoding == "rgb8":
image = image[..., ::-1]
predictions = self.predictor(image)
self.currently_busy.clear()
if "instances" in predictions:
instances = predictions["instances"].to("cpu")
boxes = np.asarray(instances.pred_boxes.tensor) if instances.has("pred_boxes") else None
if len(boxes) == 0:
return GetDetectorResultsResponse(status=ServiceStatus(), results=detections)
scores = instances.scores if instances.has("scores") else None
classes = instances.pred_classes if instances.has("pred_classes") else None
masks = np.asarray(instances.pred_masks) if instances.has("pred_masks") else [None] * len(boxes)
# if instances.has("pred_classes"):
# masks = [GenericMask(x, *x.shape) for x in np.asarray(instances.pred_masks)]
# else:
# masks = [None] * len(boxes)
for score, cls, bbox, mask in zip(scores, classes, boxes, masks):
x1, y1, x2, y2 = bbox
yv, xv = [], []
if mask is not None:
v = np.where(mask != 0)
if v:
yv, xv = v
roi = RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2)
seg_roi = SegmentOfInterest(x=xv, y=yv)
detections.objects.append(Detection(roi=roi, seg_roi=seg_roi, id=self._new_id(), track_id=-1,
confidence=score, class_name=self.classes[cls]))
except Exception as e:
print("Detectron2Server error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections)
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
try:
import numpy as np
except ImportError:
raise
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections_msg = Detections()
try:
image = ros_numpy.numpify(request.image)
if request.image.encoding == "rgb8":
image = image[..., ::-1]
self.mot.step(image)
for detection in self.mot.detections:
x1, y1, x2, y2 = detection[0][0], \
detection[0][1], \
detection[0][2], \
detection[0][3]
if detection[1] == 0:
class_ = "Ripe Strawberry"
else:
class_ = "unripe"
detections_msg.objects.append(
Detection(roi=RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2),
seg_roi=SegmentOfInterest(x=[], y=[]),
id=self._new_id(),
track_id=-1,
confidence=detection[2],
class_name=class_))
except Exception as e:
self.currently_busy.clear()
print("TensorrtServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True),
results=detections_msg)
self.currently_busy.clear()
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True),
results=detections_msg)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections = Detections()
try:
rois = []
poses = []
for pose in self.obj_poses:
transform = self.tf_buffer.lookup_transform(self.ref_frame, 'odom', rospy.Time())
pose = tf2_geometry_msgs.do_transform_pose(pose, transform).pose
x1 = pose.position.x - 0.03
x2 = pose.position.x + 0.03
y1 = pose.position.y - 0.03
y2 = pose.position.y + 0.03
z1 = pose.position.z - 0.03
z2 = pose.position.z + 0.03
roi = RegionOfInterest(x1=x1, y1=y1, z1=z1, x2=x2, y2=y2, z2=z2)
rois.append(roi)
poses.append(pose)
self.currently_busy.clear()
# Fill in the detections message
for roi, pose, track_id in zip(rois, poses, self.obj_ids):
seg_roi = SegmentOfInterest(x=[], y=[])
score = 1.0
detections.objects.append(Detection(roi=roi, seg_roi=seg_roi, pose=pose,
id=self._new_id(), track_id=track_id,
confidence=score, class_name=self.classes[0]))
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections)
except Exception as e:
print("GazeboRenderedBerriesServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
try:
import numpy as np
except ImportError:
raise
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections = Detections()
try:
image = ros_numpy.numpify(request.image)
if request.image.encoding == "rgb8":
image = image[..., ::-1]
self.mot.step(image)
for track in self.mot.visible_tracks:
confidence = 0.99
if track.label == 0:
class_name = "Ripe Strawberry"
else:
class_name = "Unripe Strawberry"
track_id = track.trk_id
roi = RegionOfInterest(x1=track.tlbr[0], y1=track.tlbr[1], x2=track.tlbr[2], y2=track.tlbr[3])
seg_roi = SegmentOfInterest(x=[], y=[])
detections.objects.append(Detection(roi=roi, seg_roi=seg_roi, id=self._new_id(), track_id=track_id,
confidence=confidence, class_name=class_name))
except Exception as e:
self.currently_busy.clear()
print("FruitCastServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections)
self.currently_busy.clear()
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=DetectionStatus(
BUSY=True))
self.currently_busy.set()
detections = Detections(header=request.image.header)
try:
image = ros_numpy.numpify(request.image)
predictions = self.predictor(image)
self.currently_busy.clear()
if "instances" in predictions:
instances = predictions["instances"].to("cpu")
boxes = np.asarray(instances.pred_boxes.tensor
) if instances.has("pred_boxes") else None
if len(boxes) == 0:
return GetDetectorResultsResponse(status=DetectionStatus(),
detections=detections)
scores = instances.scores if instances.has("scores") else None
classes = instances.pred_classes if instances.has(
"pred_classes") else None
labels = [
"{} {}".format(self.classes[cls_id], scores[idx])
for idx, cls_id in enumerate(classes)
]
masks = np.asarray(instances.pred_masks) if instances.has(
"pred_masks") else [None] * len(boxes)
# if instances.has("pred_classes"):
# masks = [GenericMask(x, *x.shape) for x in np.asarray(instances.pred_masks)]
# else:
# masks = [None] * len(boxes)
for score, cls, bbox, mask in zip(scores, classes, boxes,
masks):
x1, y1, x2, y2 = bbox
yv, xv = [], []
if mask is not None:
v = np.where(mask != 0)
if v:
yv, xv = v
roi = RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2)
seg_roi = SegmentOfInterest(x=xv, y=yv)
info = DetectionInfo(score=score,
class_id=cls,
class_name=self.classes[cls])
detections.detections.append(
Detection(roi=roi, seg_roi=seg_roi, info=info))
except Exception as e:
print("Detectron2Server error: ", e)
return GetDetectorResultsResponse(
status=DetectionStatus(ERROR=True), detections=detections)
return GetDetectorResultsResponse(status=DetectionStatus(OKAY=True),
detections=detections)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
try:
import torch
import numpy as np
from berry_segmentation.inference_color import inference_one, visualise_mask
import PIL
import cv2
from skimage import measure
except ImportError:
raise
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections = Detections()
try:
image = ros_numpy.numpify(request.image)
img = PIL.Image.fromarray(image.astype('uint8'), 'RGB')
mask = inference_one(net=self.net,
image=img,
device=self.device,
config=self.config)
for m, c in zip(mask[1:], self.config.class_names):
m = m.astype('int')
if c == 'flesh':
c = 'flesh_ripe'
# seg_roi = cv2.findContours(m, cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
contours = measure.find_contours(m, 0.5)
# for contour in contours:
# contour = np.flip(contour, axis=1)
# segmentation = contour.ravel().tolist()
if contours == []:
seg_roi = SegmentOfInterest(x=[], y=[])
roi = RegionOfInterest(x1=0, x2=0, y1=0, y2=0)
else:
y_vals = [int(coord[0]) for coord in contours[0]]
x_vals = [int(coord[1]) for coord in contours[0]]
seg_roi = SegmentOfInterest(x=x_vals, y=y_vals)
roi = RegionOfInterest(x1=min(x_vals),
y1=min(y_vals),
x2=max(x_vals),
y2=max(y_vals))
detections.objects.append(
Detection(roi=roi,
seg_roi=seg_roi,
class_name=c,
confidence=1,
id=self._new_id(),
track_id=1))
calyx_distance = 0.001 #metres
no_of_berries = 1
detections.gripper.no_of_berries = no_of_berries
detections.gripper.calyx_distance = 0.001
self.currently_busy.clear()
except Exception as e:
print("UNetServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True),
results=detections)
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True),
results=detections)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
try:
import cv2
except ImportError:
raise
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections_msg = Detections()
try:
frame = ros_numpy.numpify(request.image)
original_shape = frame.shape
frame = cv2.resize(frame, (self.image_size, int(self.image_size*0.75)))
self.darknet.copy_image_from_bytes(self.darknet_image, frame.tobytes())
detections_yolo = self.darknet.detect_image(self.network, self.class_names, self.darknet_image, thresh=0.7)
boxs = []
confidences = []
class_name= []
for detection in detections_yolo:
if detection[0] != "ripe": # only track ripe berry whose id is 0
x1, y1, x2, y2 = self._convertBack(detection[2][0], \
detection[2][1], \
detection[2][2], \
detection[2][3])
x1 = x1*original_shape[1]/frame.shape[1]
x2 = x2*original_shape[1]/frame.shape[1]
y1 = y1*original_shape[0]/frame.shape[0]
y2 = y2*original_shape[0]/frame.shape[0]
x1 = max(min(original_shape[1]-1, x1), 1)
x2 = max(min(original_shape[1]-1, x2), 1)
y1 = max(min(original_shape[0]-1, y1), 1)
y2 = max(min(original_shape[0]-1, y2), 1)
detections_msg.objects.append(Detection(roi=RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2), seg_roi=SegmentOfInterest(x=[], y=[]), id=self._new_id(), track_id=-1, confidence=float(detection[1])/100, class_name="unripe"))
continue
confidences.append(float(detection[1])/100)
class_name.append(detection[0])
bounds = detection[2]
xCoord = int(bounds[0] - bounds[2] / 2)
yCoord = int(bounds[1] - bounds[3] / 2)
boxs.append([xCoord, yCoord, int(bounds[2]), int(bounds[3])])
features = self.encoder(frame, boxs)
detections = [self.deep_detection(bbox, confidence, feature) for bbox, confidence, feature in
zip(boxs, confidences, features)]
# Run non-maxima suppression.
boxes = np.array([d.tlwh for d in detections])
scores = np.array([d.confidence for d in detections])
indices = self.preprocessing.non_max_suppression(boxes, self.nms_max_overlap, scores)
detections = [detections[i] for i in indices]
# Call the tracker
self.tracker.predict()
self.tracker.update(detections)
for track in self.tracker.tracks:
if not track.is_confirmed() or track.time_since_update > 1:
track_id = 0
continue
track_id = (int(track.track_id))
bbox = track.to_tlbr()
x1, y1, x2, y2 = bbox[0],bbox[1],bbox[2],bbox[3]
x1 = x1*original_shape[1]/frame.shape[1]
x2 = x2*original_shape[1]/frame.shape[1]
y1 = y1*original_shape[0]/frame.shape[0]
y2 = y2*original_shape[0]/frame.shape[0]
x1 = max(min(original_shape[1]-1, x1), 1)
x2 = max(min(original_shape[1]-1, x2), 1)
y1 = max(min(original_shape[0]-1, y1), 1)
y2 = max(min(original_shape[0]-1, y2), 1)
roi = (RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2))
detections_msg.objects.append(Detection(roi=roi, seg_roi=SegmentOfInterest(x=[], y=[]), id=self._new_id(), track_id=track_id,confidence=0.99, class_name="Ripe Strawberry"))
self.currently_busy.clear()
except Exception as e:
print("FruitCastServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections_msg)
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections_msg)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
try:
import numpy as np
except ImportError:
raise
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections_msg = Detections()
try:
image = ros_numpy.numpify(request.image)
if request.image.encoding == "rgb8":
image = image[..., ::-1]
boxs = []
confidences = []
class_name = []
self.mot.step(image)
for detection in self.mot.detections:
if detection[
1] != "ripe": # only track ripe berry whose class is "ripe"
x1, y1, x2, y2 = detection[0][0], \
detection[0][1], \
detection[0][2], \
detection[0][3]
detections_msg.objects.append(
Detection(roi=RegionOfInterest(x1=x1,
y1=y1,
x2=x2,
y2=y2),
seg_roi=SegmentOfInterest(x=[], y=[]),
id=self._new_id(),
track_id=-1,
confidence=detection[2],
class_name="unripe"))
continue
confidences.append(detection[2])
class_name.append(detection[1])
bounds = detection[0]
h = bounds[3] - bounds[1]
w = bounds[2] - bounds[0]
boxs.append([int(bounds[0]), int(bounds[1]), int(w), int(h)])
features = self.encoder(image, boxs)
detections = [
self.deep_detection(bbox, confidence, feature) for bbox,
confidence, feature in zip(boxs, confidences, features)
]
# Run non-maxima suppression.
boxes = np.array([d.tlwh for d in detections])
scores = np.array([d.confidence for d in detections])
indices = self.preprocessing.non_max_suppression(
boxes, self.nms_max_overlap, scores)
detections = [detections[i] for i in indices]
# Call the tracker
self.tracker.predict()
self.tracker.update(detections)
for track in self.tracker.tracks:
if not track.is_confirmed() or track.time_since_update > 1:
track_id = 0
continue
track_id = (int(track.track_id))
bbox = track.to_tlbr()
x1, y1, x2, y2 = bbox[0], bbox[1], bbox[2], bbox[3]
x1 = max(min(self.image_width - 1, x1), 1)
x2 = max(min(self.image_width - 1, x2), 1)
y1 = max(min(self.image_height - 1, y1), 1)
y2 = max(min(self.image_height - 1, y2), 1)
roi = (RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2))
detections_msg.objects.append(
Detection(roi=roi,
seg_roi=SegmentOfInterest(x=[], y=[]),
id=self._new_id(),
track_id=track_id,
confidence=0.99,
class_name="Ripe Strawberry"))
except Exception as e:
self.currently_busy.clear()
print("FruitCastServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True),
results=detections_msg)
self.currently_busy.clear()
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True),
results=detections_msg)
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
try:
import torch
from yolov5.utils.general import non_max_suppression
from yolov5.utils.general import scale_coords
from yolov5.utils.datasets import letterbox
import numpy as np
except ImportError:
raise
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections = Detections()
try:
image = ros_numpy.numpify(request.image)
if request.image.encoding == "rgb8":
image = image[..., ::-1]
original_shape = image.shape
img = letterbox(image, new_shape=self.image_size)[0]
img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB
img = np.ascontiguousarray(img)
img = torch.from_numpy(img).to(self.device)
img = img.half() if self.half else img.float() # uint8 to fp16/32
img /= 255.0 # 0 - 255 to 0.0 - 1.0
if img.ndimension() == 3:
img = img.unsqueeze(0)
with torch.no_grad():
pred = self.model(img, augment=False)[0]
pred = non_max_suppression(pred, self.conf_thresh, self.iou_thresh, agnostic=False)
for i, det in enumerate(pred):
if det is not None and len(det):
det[:, :4] = scale_coords(img.shape[2:], det[:, :4], original_shape).round()
for x1, y1, x2, y2, conf, cls in reversed(det):
x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2)
confidence = float(conf)
class_name = self.names[int(cls)]
roi = RegionOfInterest(x1=x1, y1=y1, x2=x2, y2=y2)
seg_roi = SegmentOfInterest(x=[], y=[])
detections.objects.append(Detection(roi=roi, seg_roi=seg_roi, id=self._new_id(), track_id=-1,
confidence=confidence, class_name=class_name))
self.currently_busy.clear()
except Exception as e:
print("FruitCastServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections)
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections)