Merge pull request #36 from duckietown/vehicle-detection

fix vehicle detection, thread for performance
duckietown · Nov 30, 2019 · 7695a6b · 7695a6b
2 parents 5576fc0 + b1eb4fb
commit 7695a6b
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 13 deletions.
diff --git a/packages/vehicle_detection/src/vehicle_avoidance_control_node.py b/packages/vehicle_detection/src/vehicle_avoidance_control_node.py
@@ -1,13 +1,7 @@
 #!/usr/bin/env python
-from duckietown_msgs.msg import Twist2DStamped, BoolStamped, VehiclePose, Pose2DStamped
+from duckietown_msgs.msg import Twist2DStamped, BoolStamped, VehiclePose
 
-import os
-import rospkg
 import rospy
-import yaml
-import time
-from twisted.words.protocols.oscar import CAP_SERV_REL
-from math import sqrt, sin, cos
 
 class VehicleAvoidanceControlNode(object):
 

diff --git a/packages/vehicle_detection/src/vehicle_detection_node.py b/packages/vehicle_detection/src/vehicle_detection_node.py
@@ -35,7 +35,7 @@ def __init__(self):
 
         self.lock = mutex()
         self.sub_image = rospy.Subscriber("~image", CompressedImage,
-                                          self.processImage, buff_size=921600, 
+                                          self.cbImage, buff_size=921600, 
                                           queue_size=1)
         self.sub_switch = rospy.Subscriber("~switch", BoolStamped,
                                            self.cbSwitch, queue_size=1)
@@ -59,11 +59,18 @@ def setupParam(self, param_name, default_value):
     def cbSwitch(self, switch_msg):
         self.active = switch_msg.data
 
-    def processImage(self, image_msg):
-
+    def cbImage(self, image_msg):
+        if not self.active:
+             return
+        # Start a daemon thread to process the image
+        thread = threading.Thread(target=self.processImage,args=(image_msg,))
+        thread.setDaemon(True)
+        thread.start()
+        # Returns rightaway
+
+    def processImage(self, image_msg): 
         if not self.active:
             return
-
         now = rospy.Time.now()
         if now - self.last_stamp < self.publish_duration:
             return
@@ -109,15 +116,16 @@ def processImage(self, image_msg):
             vehicle_corners_msg_out.H = self.circlepattern_dims[1]
             vehicle_corners_msg_out.W = self.circlepattern_dims[0]
             self.pub_corners.publish(vehicle_corners_msg_out)
+
         elapsed_time = (rospy.Time.now() - start).to_sec()
         self.pub_time_elapsed.publish(elapsed_time)
-        if self.publish_circles:
+
+        if detection and self.publish_circles:
             cv2.drawChessboardCorners(image_cv,
                                         self.circlepattern_dims, corners, detection)
             image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
             self.pub_circlepattern_image.publish(image_msg_out)
 
-
 if __name__ == '__main__':
     rospy.init_node('vehicle_detection', anonymous=False)
     vehicle_detection_node = VehicleDetectionNode()