[VMware-ROS1-3] ROS1과 YOLOv5 연결하기

$ roscore

$ sudo apt install ros-melodic-uvc-camera

$ rosrun uvc_camera uvc_camera_node _skip_frames:=5

$ rosrun image_view image_view image:=image_raw

$ rqt_graph

$ mkdir ros_workspace $ cd ros_workspace $ source /opt/ros/melodic/setup.bash

$ mkdir -p src/ $ cd src/ $ catkin_init_workspace Creating symlink "/home/kkim/ros_workspace/CMakeLists.txt" pointing to "/opt/ros/melodic/share/catkin/cmake/toplevel.cmake"

$ catkin_make

$ source devel/setup.bash

$ catkin_create_pkg yolov5_ros rospy roscpp std_msgs sensor_msgs message_generation

$ cd yolov5_ros/src $ git clone https://github.com/ultralytics/yolov5

$ cd yolov5 $ pip3 install --ignore-installed PyYAML $ pip3 install -r requirements.txt

$ cd ~/ros_workspace/row_ws/src/yolov5_ros $ mkdir msg

$ cd msg $ wget https://raw.githubusercontent.com/raghavauppuluri13/yolov5_pytorch_ros/master/msg/BoundingBox.msg $ wget https://raw.githubusercontent.com/raghavauppuluri13/yolov5_pytorch_ros/master/msg/BoundingBoxes.msg

$ cd ~/ros_workspace/src/yolov5_ros $ vim CMakeLists.txt ################################################ ## Declare ROS messages, services and actions ## ################################################ # 밑에 추가해주세요. 31번째 줄 즈음입니다. add_message_files( FILES BoundingBox.msg BoundingBoxes.msg ) generate_messages( DEPENDENCIES geometry_msgs sensor_msgs std_msgs ) $ cd ~/ros_workspace $ catkin_make

#!/usr/bin/env python3 # https://github.com/mats-robotics/yolov5_ros/blob/main/src/detect.py를 일부 수정하였습니다. # ------------------------------------------------------------------------------ # - import # ------------------------------------------------------------------------------ # : importing system libraries import sys import os import time # : importing externel libraries import rospy import numpy import torch import torch.backends.cudnn as cudnn # : importing - from from pathlib import Path from sensor_msgs.msg import Image from yolov5_ros.msg import BoundingBox, BoundingBoxes # : importing - yolov5 # FILE : 현재 파일의 경로 # Path(__file__) : 현재 파일의 구상 경로를 인스턴스화 # resolve() : 절대경로 반환 # - result : /home/kkim/ros_workspace/src/yolov5_ros/src/detect.py FILE = Path(__file__).resolve() # ROOT : yolov5 폴더의 경로 # - yolov5 폴더의 path를 구한 뒤 sys.path에 append하여 import 가능하도록 설정 ROOT = FILE.parents[0] / "yolov5" if str(ROOT) not in sys.path: sys.path.append(str(ROOT)) # - 상대 경로로 변환 ROOT = Path(os.path.relpath(ROOT, Path.cwd())) # main importing from models.common import DetectMultiBackend from utils.general import ( check_img_size, non_max_suppression, scale_boxes) from utils.torch_utils import select_device from utils.augmentations import letterbox # ------------------------------------------------------------------------------ # - defines # ------------------------------------------------------------------------------ IMG_W = 640 IMG_H = 480 # ------------------------------------------------------------------------------ # - class : Yolov5Detector # ------------------------------------------------------------------------------ def param(_name, _value): return rospy.get_param(_name, _value) class Yolov5Detector: def __init__(self): # print("Yolov5 __init__") # get rospy parameters self.cnf_thr = param("~confidence_threshold", 0.75) self.iou_thr = param("~iou_threshold", 0.45) self.agn_nms = param("~agnostic_nms", True) self.max_det = param("~maximum_detections", 1000) self.classes = param("~classes", None) self.dnn = param("~dnn", True) # initialize weights self.weight = param("~weights", "yolov5s.pt") # initialize model self.device = select_device(str(param("~device", "cpu"))) self.model = DetectMultiBackend( self.weight, device = self.device, dnn = self.dnn) model = self.model _, self.stride, self.names, self.pt, self.jit, self.onnx, self.engine = (None, model.stride, model.names, model.pt, model.jit, model.onnx, model.engine) # setting inference size self.img_siz = [ param("~inference_size_w", IMG_W), param("~inference_size_h", IMG_H) ] self.img_siz = check_img_size(self.img_siz, s=self.stride) # half self.half = param("~half", False) self.half &= ( self.pt or self.jit or self.onnx or self.engine) and self.device.type != "cpu" if self.pt or self.jit: self.model.model.half() if self.half else self.model.model.float() batch_size = 1 # set True to speed up constant image size inference cudnn.benchmark = True self.model.warmup(imgsz=(1 if self.pt else batch_size, 3, *self.img_siz)) # initialize subscriber to image topic rospy.Subscriber('/image_raw', Image, self.callback) self.pre_pub = rospy.Publisher( rospy.get_param("~output_topic", "/yolov5_ros"), BoundingBoxes, queue_size=10) def callback(self, _img_msg): # print("Yolov5 callback") typ = numpy.dtype("uint8") typ = typ.newbyteorder('>' if _img_msg.is_bigendian else '<') img_cv = numpy.ndarray( shape=( _img_msg.height, _img_msg.width, 3), dtype=typ, buffer=_img_msg.data) if _img_msg.is_bigendian == (sys.byteorder == 'little'): img_cv = img_cv.byteswap().newbyteorder() img, img_0 = self.preprocess(img_cv) img = torch.from_numpy(img) img = img.half() if self.half else img.float() img /= 255 if len(img.shape) == 3: img = img[None] pre = self.model(img, augment=False, visualize=False) pre = non_max_suppression(pre, self.cnf_thr, self.iou_thr, self.classes, max_det = self.max_det) det = pre[0].cpu().numpy() box = BoundingBoxes() box.header = _img_msg.header box.image_header= _img_msg.header if len(det): det[:, :4] = scale_boxes(img.shape[2:], det[:, :4], img_0.shape).round() for *xyxy, conf, cls in reversed(det): c = int(cls) box_msg = BoundingBox() box_msg.Class = self.names[c] box_msg.probability = conf box_msg.xmin = int(xyxy[0]) box_msg.ymin = int(xyxy[1]) box_msg.xmax = int(xyxy[2]) box_msg.ymax = int(xyxy[3]) box.bounding_boxes.append(box_msg) self.pre_pub.publish(box) def preprocess(self, _img): _cpy = _img.copy() _img = numpy.array( [letterbox( _img, self.img_siz, stride=self.stride, auto=self.pt)[0]]) _img = _img[..., ::-1].transpose((0, 3, 1, 2)) _img = numpy.ascontiguousarray(_img) return _img, _cpy if __name__ == "__main__": # print("MAIN start") rospy.init_node("yolov5", anonymous=True) detector = Yolov5Detector() rospy.spin()

$ cd ~/ros_workspace/src/yolov5_ros/src $ chmod +x detect.py

$ cd ~/ros_workspace/src/yolov5_ros $ vim CMakeLists.txt ############# ## Install ## ############# # ... # 밑에 추가해주세요. catkin_install_python(PROGRAMS src/detect.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )

$ cd ~/ros_workspace $ catkin_make $ source devel/setup.bash $ rosrun yolov5_ros detect.py

$ rqt_graph

$ cd ~/ros_workspace/src $ catkin_create_pkg robot_ctrl rospy roscpp std_msgs sensor_msgs yolov5_ros

#!/usr/bin/env python3 # ------------------------------------------------------------------------------ # - import # ------------------------------------------------------------------------------ # : importing system libraries import sys # : importing externel libraries import numpy import cv2 import rospy import message_filter # : importing - from from sensor_msgs.msg import Image from yolov5_ros.msg import BoundingBoxes # ------------------------------------------------------------------------------ # - class: robotctrl # ------------------------------------------------------------------------------ class RobotControl: def __init__(self): img_cb = message_filter.Subscriber('/image_raw', Image) yolov5 = message_filter.Subscriber('/yolov5_ros', BoundingBoxes) timsyn = message_filter.TimeSynchronizer([img_cb, yolov5], 10) timsyn.registerCallback(self.callback) def callback(self, _img_msg, _box_msg): typ = numpy.dtype("uint8") typ = typ.newbyteorder('>' if _img_msg.is_bigendian else '<') img = numpy.ndarray( shape =( _img_msg.height, _img_msg.width, 3), dtype =typ, buffer=_img_msg.data) if _img_msg.is_bigendian == (sys.byteorder == 'little'): img = img.byteswap().newbyteorder() img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) for box in _box_msg.bounding_boxes: print(box) cv2.rectangle( img, (box.xmin, box.ymin), (box.xmax, box.ymax), (255, 0, 0), 2) cv2.imwrite('tmp.jpg', img) if __name__ == "__main__": rospy.init_node("robot_control", anonymous=True) control = RobotControl() rospy.spin()

$ cd ~/ros_workspace/src/robot_ctrl/src $ chmod +x control.py

$ cd ~/ros_workspace/src/robot_ctrl $ vim CMakeLists.txt ############# ## Install ## ############# # ... # 밑에 추가해주세요. catkin_install_python(PROGRAMS src/control.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )

$ cd ~/ros_workspace $ catkin_make $ source devel/setup.bash $ rosrun yolov5_ros control.py