ROS build and exec ok

Author: tzuyichiu

ros_ws/src/detection/CMakeLists.txt

@@ -32,6 +32,7 @@ generate_messages(
 ## catkin specific configuration ##
 ###################################
 
+catkin_package()
 
 ###########
 ## Build ##

ros_ws/src/detection/msg/Detection.msg

@@ -17,4 +17,4 @@ float32 h
 # class
 uint8 cls
 
-float32 confidence
+float32 confidence

ros_ws/src/detection/msg/Detections.msg

@@ -1,8 +1,5 @@
 # Detections.msg
 ## List of detected bounding boxes
 
-Header header
-Detection[] detections
-
-# frame
-uint8 frame
+# Header header
+Detection[] detections

ros_ws/src/tracking/scripts/detection_test.py

@@ -7,13 +7,15 @@
 
 import os
 import cv2
+from urllib.request import urlretrieve
 import numpy as np
 from motpy.tracker import Tracker
 from detection.msg import Detections, Detection as DetectionROS
+import rospy
 
 PROJECT_FOLDER = os.path.dirname(os.path.realpath(__file__))
 DATASET_FOLDER = os.path.join(PROJECT_FOLDER, 'TP3_data')
-VIDEO_FOLDER = os.path.join(DATASET_FOLDER, 'frames')
+VIDEO_FOLDER = os.path.join(DATASET_FOLDER, 'TP3_data', 'frames')
 YOLO_FOLDER = os.path.join(PROJECT_FOLDER, 'yolo')
 WEIGHTS_URL = 'https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights'
 WEIGHTS_PATH = os.path.join(YOLO_FOLDER, 'yolov4.weights')
@@ -24,10 +26,10 @@
 if not os.path.exists(YOLO_FOLDER):
     os.mkdir(YOLO_FOLDER)
 if not os.path.isfile(WEIGHTS_PATH):
-    logger.debug('downloading weights to' + WEIGHTS_PATH)
+    print('downloading weights to' + WEIGHTS_PATH)
     urlretrieve(WEIGHTS_URL, WEIGHTS_PATH)
 if not os.path.isfile(CONFIG_PATH):
-    logger.debug('downloading config to' + CONFIG_PATH)
+    print('downloading config to' + CONFIG_PATH)
     urlretrieve(CONFIG_URL, CONFIG_PATH)
 net = cv2.dnn.readNetFromDarknet(CONFIG_PATH, WEIGHTS_PATH)
 
@@ -40,8 +42,8 @@ def init_detections():
     for line in lines:
         x, xmax, y, ymax = map(int, line.split()[2:6])
         detection = DetectionROS(x=x, y=y, w=xmax-x, h=ymax-y, confidence=1)
-        detections.append(Detection(box=box,))
-    return Detections(detections=detections, frame=1)
+        detections.append(detection)
+    return Detections(detections=detections)
 
 def yolo_detections(frame, img, threshold=0.3):
     """Generates bbox detections with yolo-v4.
@@ -52,7 +54,7 @@ def yolo_detections(frame, img, threshold=0.3):
     blob = cv2.dnn.blobFromImage(img, 1/255.0, (416, 416), swapRB=True, crop=False)
     net.setInput(blob)
     ln = net.getLayerNames()
-    ln = [ln[i[0] - 1] for i in net.getUnconnectedOutLayers()]
+    ln = [ln[i - 1] for i in net.getUnconnectedOutLayers()]
     layer_outputs = net.forward(ln)
 
     detections = []
@@ -68,13 +70,13 @@ def yolo_detections(frame, img, threshold=0.3):
                 y = int(centerY - (h / 2))
                 detection = DetectionROS(x=x, y=y, w=w, h=h, confidence=conf)
                 detections.append(detection)
-    return Detections(detections=detections, frame=frame)
+    return Detections(detections=detections)
 
 def main():
     init = init_detections()
     pub = rospy.Publisher('detections', Detections, queue_size=1)
     rospy.init_node('detection_test', anonymous=False)
-    r = rospy.Rate(24)
+    r = rospy.Rate(1)
     nbframe = len(os.listdir(VIDEO_FOLDER))
     imgs = (cv2.imread(os.path.join(VIDEO_FOLDER, f'frame{frame}.jpg')) 
                 for frame in range(1, nbframe+1))
@@ -90,7 +92,7 @@ def main():
                 detections = yolo_detections(frame, img)
             except StopIteration:
                 break
-        rospy.spin()
+        rospy.loginfo(detections)
         pub.publish(detections)
         r.sleep()
 

ros_ws/src/tracking/scripts/track.py

@@ -15,14 +15,12 @@
 
 from motpy import MultiObjectTracker, Track, Detection as DetectionMOT
 
-dt = 1/24 # TODO : changer
+dt = 1 # TODO : changer
 
 class Tracker:
     def __init__(self):
         # ROS
-        self.sub = rospy.Subscriber(
-            'detections_in', Detections, self.detections_callback, queue_size=1)
-
+        rospy.init_node('tracking', anonymous=False)
         self.pub = rospy.Publisher('tracks', Tracklets, queue_size=1)
 
         # motpy - no confidence with the motion model (0.1)
@@ -38,16 +36,17 @@ def detections_callback(self, detections_ros: Detections) -> Tracklets:
         detections_mot = [self.det_ros2mot(detection_ros) 
                             for detection_ros in detections_ros.detections]
         tracks = self.mot.step(detections_mot)
-        tracklets = Tracklets([self.trk_mot2ros(track) for track in tracks])
+        tracks = [self.trk_mot2ros(track) for track in tracks]
+        tracklets = Tracklets(tracklets=tracks)
         self.pub.publish(tracklets)
-
-        frame = detections_ros.frame
-        img = cv2.imread(os.path.join(VIDEO_FOLDER, f'frame{frame}.jpg'))
-        for det in detections_mot:
-            img = draw_rectangle(img, det.box, color=(0, 255, 0), thickness=2)
-        for trk in tracks:
-            img = draw_rectangle(img, det.box, color=(255, 0, 0), thickness=5)
-        cv2.imshow('Main', cv2.resize(img, (960, 540)))
+        rospy.loginfo(tracklets)
+        return tracklets
+        #img = cv2.imread(os.path.join(VIDEO_FOLDER, f'frame{frame}.jpg'))
+        #for det in detections_mot:
+        #    img = draw_rectangle(img, det.box, color=(0, 255, 0), thickness=2)
+        #for trk in tracks:
+        #    img = draw_rectangle(img, det.box, color=(255, 0, 0), thickness=5)
+        #cv2.imshow('Main', cv2.resize(img, (960, 540)))
 
     def det_ros2mot(self, detection: DetectionROS) -> DetectionMOT:
         box = np.array([detection.x, detection.y, 
@@ -59,8 +58,8 @@ def trk_mot2ros(self, track: Track) -> Tracklet:
         return tracklet
 
 def main():            
-    rospy.init_node('tracking', anonymous=False)
     tracker = Tracker()
+    rospy.Subscriber('detections', Detections, tracker.detections_callback)
     while not rospy.is_shutdown():
         rospy.spin()