diff --git a/minibot_vision/scripts/SegmentSign.py b/minibot_vision/scripts/SegmentSign.py index 3f49542a804072ef578b0af2b973b0e0ad6d0fdd..de6587a4b03f7207a1e89441e6763f18de067cd4 100644 --- a/minibot_vision/scripts/SegmentSign.py +++ b/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") diff --git a/minibot_vision/scripts/SignDetector.py b/minibot_vision/scripts/SignDetector.py index b30648f035d35f84c3c896755b187ed25deaaf17..9c813fd6b356570595fe527c421a450fe626faf7 100755 --- a/minibot_vision/scripts/SignDetector.py +++ b/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)