made image patch publish faster

minibot_vision/scripts/SegmentSign.py

@@ -56,7 +56,11 @@ def fetch_rosparams():
     global canny, accum_thresh, ZOOM_THREASHOLD, MIN_DEPTH, MAX_DEPTH, MIN_RADIUS, MAX_RADIUS
 
     canny = rospy.get_param("sign_detector/canny_param1")
+    if canny < 1:
+        canny = 1
     accum_thresh = rospy.get_param("sign_detector/canny_param2")
+    if accum_thresh < 1:
+        accum_thresh = 1
     ZOOM_THREASHOLD = rospy.get_param("sign_detector/zoom_threshold")
 
     MIN_DEPTH = rospy.get_param("sign_detector/min_depth")

minibot_vision/scripts/SignDetector.py

@@ -15,6 +15,8 @@ from copy import copy
 from vision_msgs.msg import Detection2D, ObjectHypothesisWithPose
 from minibot_msgs.srv import set_url
 import time
+from threading import Thread
+
 
 # *** CONSTANTS ***
 visualize = True
@@ -39,6 +41,7 @@ enable_capture_images = False
 toggle_hough_detection = True
 pub_raw_img = None
 pub_cmpr_img = None
+pub_img_patch_rate = None
 
 # subscribe to RGB img
 def image_color_callback(data):
@@ -128,9 +131,9 @@ def detect_sign(img_rgb_stream, image_timestamp):
     patches = SegmentSign.get_tensor_patches(img_orig, keypoints, zoom=toggle_hough_detection)
 
     # publish patch for capture images
-    if enable_capture_images:
-        if len(patches) >= 1:
-            publish_img_patch(patches[0])
+    #if enable_capture_images:
+    #    if len(patches) >= 1:
+    #        publish_img_patch(patches[0])
 
     # cut to multiple images at keypoints
     text = []
@@ -179,9 +182,13 @@ def set_visualize_callback(req):
 
 
 def enable_capture_callback(req):
-    global enable_capture_images
+    global enable_capture_images, pub_img_patch_rate
 
     enable_capture_images = req.data
+    if enable_capture_images:
+        pub_img_patch_rate = rospy.Rate(15)
+    else:
+        pub_img_patch_rate = rospy.Rate(5)
     rospy.loginfo("({}) set enable_capture_images to {}".format(rospy.get_name(), enable_capture_images))
 
     return True, ""
@@ -212,6 +219,25 @@ def update_rosparams_callback(req):
     return True, ""
 
 
+def pub_image_patch_thread():
+    global pub_img_patch_rate, img_depth_stream, img_rgb_stream, enable_capture_images
+
+    pub_img_patch_rate = rospy.Rate(5)
+
+    while not rospy.is_shutdown():
+        if enable_capture_images:
+            img_orig = np.copy(img_rgb_stream)
+
+            keypoints = hough_pipeline(img_orig, img_depth_stream)
+            patches = SegmentSign.get_tensor_patches(img_orig, keypoints, zoom=toggle_hough_detection)
+
+            # publish patch for capture images
+            if len(patches) >= 1:
+                publish_img_patch(patches[0])
+
+        pub_img_patch_rate.sleep()
+
+
 if __name__ == "__main__":
     rospy.init_node("sign_detector")
 
@@ -241,6 +267,10 @@ if __name__ == "__main__":
 
     rospy.loginfo("({}) Sign classification running with params fps: {}, visualization_compression_factor: {}".format(rospy.get_name(), fps, visualization_compression_factor))
 
+    pub_image_patch_worker = Thread(target=pub_image_patch_thread)
+    pub_image_patch_worker.deamon = True
+    pub_image_patch_worker.start()
+
     rate = rospy.Rate(fps)
     while not rospy.is_shutdown():
         detect_sign(img_rgb_stream, img_rgb_timestamp)