# Subscribe to input image topic. # object_detector ([std_msgs::Int8]) # Publishes the number of detected objects. # bounding_boxes ([darknet_ros_msgs::BoundingBoxes]) # Publishes an array of bounding boxes that gives information of the position and size of the bounding box in pixel coordinates. # detection_image ([sensor_msgs::Image]) # Publishes an image of the detection image including the bounding boxes. import rclpy from rclpy.node import Node from std_msgs.msg import String from sensor_msgs.msg import Image from cv_bridge import CvBridge import argparse import os import sys from pathlib import Path import cv2 import torch import torch.backends.cudnn as cudnn FILE = Path(__file__).resolve() ROOT = FILE.parents[0] # root directory if str(ROOT) not in sys.path: sys.path.append(str(ROOT)) # add ROOT to PATH ROOT = Path(os.path.relpath(ROOT, Path.cwd())) # relative from models.common import DetectMultiBackend from utils.datasets import IMG_FORMATS, VID_FORMATS, LoadImages, LoadStreams from utils.general import (LOGGER, check_file, check_img_size, check_imshow, check_requirements, colorstr, increment_path, non_max_suppression, print_args, scale_coords, strip_optimizer, xyxy2xywh) from utils.plots import Annotator, colors, save_one_box from utils.torch_utils import select_device, time_sync from utils.augmentations import Albumentations, augment_hsv, copy_paste, letterbox import numpy as np from yolov3_msg.msg import BoundingBox, BoundingBoxes from rclpy.clock import Clock from sensor_msgs.msg import CameraInfo @torch.no_grad() def run(weights=ROOT / 'yolov3.pt', # model.pt path(s) source=ROOT / 'data/images', # file/dir/URL/glob, 0 for webcam imgsz=1280, # inference size (pixels) conf_thres=0.25, # confidence threshold iou_thres=0.45, # NMS IOU threshold max_det=1000, # maximum detections per image device='', # cuda device, i.e. 0 or 0,1,2,3 or cpu view_img=False, # show results save_txt=False, # save results to *.txt save_conf=False, # save confidences in --save-txt labels save_crop=False, # save cropped prediction boxes nosave=False, # do not save images/videos classes=None, # filter by class: --class 0, or --class 0 2 3 agnostic_nms=False, # class-agnostic NMS augment=False, # augmented inference visualize=False, # visualize features update=False, # update all models project=ROOT / 'runs/detect', # save results to project/name name='exp', # save results to project/name exist_ok=False, # existing project/name ok, do not increment line_thickness=3, # bounding box thickness (pixels) hide_labels=False, # hide labels hide_conf=False, # hide confidences half=False, # use FP16 half-precision inference dnn=False, # use OpenCV DNN for ONNX inference ): # Directories save_dir = increment_path(Path(project) / name, exist_ok=exist_ok) # increment run (save_dir / 'labels' if save_txt else save_dir).mkdir(parents=True, exist_ok=True) # make dir # Load model device = select_device(device) model = DetectMultiBackend(weights, device=device, dnn=dnn) stride, names, pt, jit, onnx = model.stride, model.names, model.pt, model.jit, model.onnx imgsz = check_img_size(imgsz, s=stride) # check image size # Half half &= pt and device.type != 'cpu' # half precision only supported by PyTorch on CUDA if pt: model.model.half() if half else model.model.float() # Padded resize dataset_img = letterbox(source, imgsz, stride=stride, auto=pt)[0] # Convert dataset_img = dataset_img.transpose((2, 0, 1))[::-1] # HWC to CHW, BGR to RGB dataset_img = np.ascontiguousarray(dataset_img) # Run inference if pt and device.type != 'cpu': model(torch.zeros(1, 3, *imgsz).to(device).type_as(next(model.model.parameters()))) # warmup dt, seen = [0.0, 0.0, 0.0], 0 bs = 1 t1 = time_sync() im = dataset_img im = torch.from_numpy(im).to(device) im = im.half() if half else im.float() # uint8 to fp16/32 im /= 255 # 0 - 255 to 0.0 - 1.0 if len(im.shape) == 3: im = im[None] # expand for batch dim t2 = time_sync() dt[0] += t2 - t1 pred = model(im, augment=augment, visualize=visualize) t3 = time_sync() pred = non_max_suppression(pred, conf_thres, iou_thres, classes, agnostic_nms, max_det=max_det) dt[2] += time_sync() - t3 ## iterate through detections for i, det in enumerate(pred): seen += 1 path = '/home/parallels' p, im0, frame = path, source.copy(), 'frame' p = Path(p) # to Path save_path = '/home/parallels/predict.jpeg' annotator = Annotator(im0, line_width=line_thickness, example=str(names)) time_stamp = Clock().now() bounding_boxes_msg = BoundingBoxes() bounding_boxes_msg.header.stamp = time_stamp.to_msg() bounding_boxes_msg.header.frame_id = 'detection' s = '' if len(det): # Rescale boxes from img_size to im0 size det[:, :4] = scale_coords(im.shape[2:], det[:, :4], im0.shape).round() # Print results number_of_uniq_det = {} for c in det[:, -1].unique(): n = (det[:, -1] == c).sum() # detections per class s += f"{n} {names[int(c)]}{'s' * (n > 1)}, " # add to string number_of_uniq_det[names[int(c)]] = int(n) # Write results append_count = 0 last_name = "" for *xyxy, conf, cls in reversed(det): bounding_box = BoundingBox() c = int(cls) # integer class label = None if hide_labels else (names[c] if hide_conf else f'{names[c]} {conf:.2f}') annotator.box_label(xyxy, label, color=colors(c, True)) bounding_box.probability = float(conf) bounding_box.xmin = int(xyxy[0]) bounding_box.ymin = int(xyxy[1]) bounding_box.xmax = int(xyxy[2]) bounding_box.ymax = int(xyxy[3]) bounding_box.id = c if last_name != names[c]: append_count = 0 if number_of_uniq_det[names[c]] > 1: bounding_box.class_id = names[c] + "_" + str(append_count) append_count+=1 last_name = names[c] else: bounding_box.class_id = names[c] append_count = 0 bounding_boxes_msg.bounding_boxes.append(bounding_box) # Print time (inference-only) LOGGER.info(f'{s}Done. ({t3 - t2:.3f}s)') # Stream results im0 = annotator.result() if update: strip_optimizer(weights) # update model (to fix SourceChangeWarning) # cv2.imwrite(save_path, im0) return im0, bounding_boxes_msg class DetectOnBag(Node): def __init__(self): super().__init__('detect_on_bag') self.camera_info = None self.depth_img = None self.subscription = self.create_subscription( Image, '/camera/color/image_raw', self.image_callback, 10) self.subscription # prevent unused variable warning self.subscription = self.create_subscription( Image, '/camera/aligned_depth_to_color/image_raw', self.image_depth_callback, 10) self.subscription self.subscription = self.create_subscription( CameraInfo, '/camera/color/camera_info', self.camera_info_callback, 10) self.subscription self.detected_img_pub = self.create_publisher(Image, '/detection_image', 10) self.bboxes_pub = self.create_publisher(BoundingBoxes, '/bboxes', 10) self.depth_pub = self.create_publisher(Image, '/depth_img', 10) self.img_pub = self.create_publisher(Image, '/rgb_img', 10) self.info_pub = self.create_publisher(CameraInfo, '/camera_info', 10) def camera_info_callback(self, msg): print("camera info cb") # print(msg.header.stamp) self.camera_info = msg def image_depth_callback(self, msg): print("depth info cb") # print(msg.header.stamp) self.depth_img = msg def image_callback(self, msg): print("rgb img cb") # print(msg.header.stamp) bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') detected_img, bboxes = run('src/yolov3/runs/train/exp15/weights/best.pt', cv_image) self.detected_img_pub.publish(self.numpy_array_to_image_msg(detected_img)) bboxes.image_header = msg.header bboxes.header.stamp = msg.header.stamp bboxes.header.frame_id = 'detection' self.bboxes_pub.publish(bboxes) if self.depth_img: self.depth_pub.publish(self.depth_img) self.img_pub.publish(msg) if self.camera_info: self.info_pub.publish(self.camera_info) def numpy_array_to_image_msg(self, numpy_array): # Create a CvBridge object bridge = CvBridge() # Convert the numpy array to a cv image cv_image = cv2.cvtColor(numpy_array, cv2.COLOR_RGB2BGR) # Convert the cv image to a ROS message ros_image = bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") return ros_image def main(args=None): rclpy.init(args=args) detect_on_bag = DetectOnBag() rclpy.spin(detect_on_bag) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) detect_on_bag.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()