Build with ROS & A detect service with test image send client

.gitignore

@@ -64,3 +64,4 @@ uselib
 uselib_track
 darknet
 vcpkg/
+build/

CMakeLists.txt

@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 3.18)
 include(CMakeDependentOption)
 
+option(BUILD_ROS "Build ROS package" ON)
 set(Darknet_MAJOR_VERSION 0)
 set(Darknet_MINOR_VERSION 2)
 set(Darknet_PATCH_VERSION 5)
@@ -561,41 +562,48 @@ install(EXPORT DarknetTargets
 # Export the package for use from the build-tree (this registers the build-tree with a global CMake-registry)
 export(PACKAGE Darknet)
 
-# ROS catkin
-find_package(catkin REQUIRED COMPONENTS
-  sensor_msgs
-  cv_bridge
-  image_transport
-  nodelet
-  roscpp
-  message_generation
-  message_runtime
-)
+if (BUILD_ROS)
+  # ROS catkin
+  find_package(catkin REQUIRED COMPONENTS
+    sensor_msgs
+    cv_bridge
+    image_transport
+    nodelet
+    roscpp
+    message_generation
+    message_runtime
+  )
 
-add_message_files(
-  FILES
-  BBox.msg
-  DetectResult.msg
-)
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-)
+  add_message_files(
+    FILES
+    BBox.msg
+    DetectResult.msg
+  )
+  add_service_files(
+    FILES
+    Detect.srv
+  )
+  generate_messages(
+    DEPENDENCIES
+    std_msgs
+    sensor_msgs
+  )
 
-catkin_package(
-  INCLUDE_DIRS include ${catkin_INCLUDE_DIRS}
-  LIBRARIES ${OpenCV_LIBS}
-  CATKIN_DEPENDS nodelet sensor_msgs cv_bridge image_transport
-)
+  catkin_package(
+    INCLUDE_DIRS include ${catkin_INCLUDE_DIRS}
+    LIBRARIES ${OpenCV_LIBS}
+    CATKIN_DEPENDS nodelet sensor_msgs cv_bridge image_transport
+  )
 
-include_directories(
-  include
-  src
-  ${catkin_INCLUDE_DIRS}
-  ${OpenCV_INDLUDE_DIRS}
-)
-add_library(detect_nodelet ${CMAKE_CURRENT_LIST_DIR}/src/nodelet_detector.cpp)
-target_link_libraries(detect_nodelet dark ${catkin_LIBRARIES})
+  include_directories(
+    include
+    src
+    ${catkin_INCLUDE_DIRS}
+    ${OpenCV_INDLUDE_DIRS}
+  )
+  # add_library(detect_nodelet ${CMAKE_CURRENT_LIST_DIR}/src/nodelet_detector.cpp)
+  # target_link_libraries(detect_nodelet dark ${catkin_LIBRARIES})
+endif ()
 
 # Create the DarknetConfig.cmake
 # First of all we compute the relative path between the cmake config file and the include path

detect_service.py

@@ -0,0 +1,129 @@
+import rospy
+import os
+from detector.srv import Detect, DetectRequest, DetectResponse
+from detector.msg import BBox, DetectResult
+import argparse
+import os
+import random
+import cv2
+import darknet
+from functools import partial
+from cv_bridge import CvBridge
+
+
+def parser():
+    parser = argparse.ArgumentParser(description="YOLO Object Detection")
+    parser.add_argument("--input",
+                        type=str,
+                        default="",
+                        help="image source. It can be a single image, a"
+                        "txt with paths to them, or a folder. Image valid"
+                        " formats are jpg, jpeg or png."
+                        "If no input is given, ")
+    parser.add_argument(
+        "--batch_size",
+        default=1,
+        type=int,
+        help="number of images to be processed at the same time")
+    parser.add_argument("--weights",
+                        default="yolov4.weights",
+                        help="yolo weights path")
+    parser.add_argument("--ext_output",
+                        action='store_true',
+                        help="display bbox coordinates of detected objects")
+    parser.add_argument(
+        "--save_labels",
+        action='store_true',
+        help="save detections bbox for each image in yolo format")
+    parser.add_argument("--config_file",
+                        default="./cfg/yolov4.cfg",
+                        help="path to config file")
+    parser.add_argument("--data_file",
+                        default="./cfg/coco.data",
+                        help="path to data file")
+    parser.add_argument("--thresh",
+                        type=float,
+                        default=.25,
+                        help="remove detections with lower confidence")
+    return parser.parse_args()
+
+
+def check_arguments_errors(args):
+    assert 0 < args.thresh < 1, "Threshold should be a float between zero and one (non-inclusive)"
+    if not os.path.exists(args.config_file):
+        raise (ValueError("Invalid config path {}".format(
+            os.path.abspath(args.config_file))))
+    if not os.path.exists(args.weights):
+        raise (ValueError("Invalid weight path {}".format(
+            os.path.abspath(args.weights))))
+    if not os.path.exists(args.data_file):
+        raise (ValueError("Invalid data file path {}".format(
+            os.path.abspath(args.data_file))))
+    if args.input and not os.path.exists(args.input):
+        raise (ValueError("Invalid image path {}".format(
+            os.path.abspath(args.input))))
+
+
+def image_detection(image, network, class_names, thresh):
+    # Darknet doesn't accept numpy images.
+    # Create one with image we reuse for each detect
+    width = darknet.network_width(network)
+    height = darknet.network_height(network)
+    darknet_image = darknet.make_image(width, height, 3)
+
+    image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+    image_resized = cv2.resize(image_rgb, (width, height),
+                               interpolation=cv2.INTER_LINEAR)
+
+    darknet.copy_image_from_bytes(darknet_image, image_resized.tobytes())
+    detections = darknet.detect_image(network,
+                                      class_names,
+                                      darknet_image,
+                                      thresh=thresh)
+    darknet.free_image(darknet_image)
+    return detections
+
+
+def handle_image(req: DetectRequest, network, class_names, thresh):
+    # print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
+
+    image = CvBridge().imgmsg_to_cv2(req.image)
+    # prev_time = time.time()
+    # [(label_name, prob(%), x, y, w, h)]
+    detections = image_detection(image, network, class_names, thresh)
+    # darknet.print_detections(detections, args.ext_output)
+    # fps = int(1/(time.time() - prev_time))
+    # print("FPS: {}".format(fps))
+    bboxes = []
+    for det in detections:
+        # label, x_min ,x_max, y_min, y_max, prob
+        box = BBox(0, det[2][0], det[2][1], det[2][0] + det[2][2],
+                   det[2][1] + det[2][3], float(det[1]))
+        bboxes.append(box)
+    result = DetectResult(0, len(bboxes), bboxes)
+
+    return DetectResponse(result)
+
+
+def main():
+    rospy.init_node('detect_service')
+
+    args = parser()
+    check_arguments_errors(args)
+
+    random.seed(3)  # deterministic bbox colors
+    network, class_names, _ = darknet.load_network(args.config_file,
+                                                   args.data_file,
+                                                   args.weights,
+                                                   batch_size=args.batch_size)
+
+    handler = partial(handle_image,
+                      network=network,
+                      class_names=class_names,
+                      thresh=args.thresh)
+    s = rospy.Service('detect_service', Detect, handler)
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()

srv/Detect.srv

@@ -0,0 +1,3 @@
+sensor_msgs/Image image
+---
+detector/DetectResult result

test/test_service.py

@@ -0,0 +1,27 @@
+import rospy
+import time
+from detector.srv import DetectRequest, Detect
+import cv2
+from cv_bridge import CvBridge, CvBridgeError
+
+serv_name = 'detect_service'
+
+def send_image(img):
+    rospy.wait_for_service(serv_name)
+    try:
+        detect = rospy.ServiceProxy(serv_name, Detect)
+        msg = DetectRequest(img)
+        resp1 = detect(msg)
+        print(resp1.result)
+    except rospy.ServiceException as e:
+        print("Service call failed: %s"%e)
+
+rospy.init_node("send_image")
+
+image = cv2.imread('data/dog.jpg')
+bridge = CvBridge()
+img_msg = bridge.cv2_to_imgmsg(image)
+
+while True:
+    send_image(img_msg)
+    time.sleep(1)