SegmentSign: taking img res params now directly out of image

minibot_vision/README.md

@@ -23,9 +23,9 @@ The following hotkeys are supported:
 The functionality of this tool is also available via ros.
 The two hyper-params are on the param server as `sign_detector/canny_param1` and `sign_detector/canny_param2` 
 and represent param1 and param2 of HoughCircles() at https://docs.opencv.org/3.4/dd/d1a/group__imgproc__feature.html#ga47849c3be0d0406ad3ca45db65a25d2d.
-- **crop_sign/enable** (service): enables the functionality of the tool.
-- **crop_sign/command** (service): A service to reflect the functionality described above. See `minibot_msgs/srv/segment_sign_command.srv`.
-- **crop_sign/result_image/compressed** (topic): A compressed version of the resulting image, 
+- **hough_transform/enable** (service): enables the functionality of the tool.
+- **hough_transform/command** (service): A service to reflect the functionality described above. See `minibot_msgs/srv/segment_sign_command.srv`.
+- **hough_transform/result_image/compressed** (topic): A compressed version of the resulting image, 
   so an image with some information what the sign segmentation is doing.
 
 ## Guideline for optimal Sign Detection

minibot_vision/scripts/SegmentSign.py

@@ -42,15 +42,23 @@ def empty(d):
     pass
 
 
-def update_camera_params():
-    global IMG_RES, DEPTH_RES
-
-    cam_color_info = rospy.wait_for_message("camera/color/camera_info", CameraInfo)
-    cam_depth_info = rospy.wait_for_message("camera/depth/camera_info", CameraInfo)
-
-    IMG_RES = (cam_color_info.height, cam_color_info.width)
-    DEPTH_RES = (cam_depth_info.height, cam_depth_info.width)
-
+# def update_camera_params():
+#     global IMG_RES, DEPTH_RES
+#
+#     topic_name = "camera/color/camera_info"
+#     try:
+#         cam_color_info = rospy.wait_for_message(topic_name, CameraInfo, timeout=10)
+#         IMG_RES = (cam_color_info.height, cam_color_info.width)
+#     except rospy.ROSException:
+#         rospy.logwarn("Camera parameters from topic {} not received after timeout. Using standard params".format(topic_name))
+#
+#     topic_name = "camera/depth/camera_info"
+#     try:
+#         cam_depth_info = rospy.wait_for_message(topic_name, CameraInfo, timeout=10)
+#         DEPTH_RES = (cam_depth_info.height, cam_depth_info.width)
+#     except rospy.ROSException:
+#         rospy.logwarn("Camera parameters from topic {} not received after timeout. Using standard params".format(topic_name))
+#
 
 def fetch_rosparams():
     global canny, accum_thresh, ZOOM_THREASHOLD, MIN_DEPTH, MAX_DEPTH, MIN_RADIUS, MAX_RADIUS
@@ -75,6 +83,7 @@ def image_color_callback(data):
 
     try:
         img_rgb_stream = cv_bridge.imgmsg_to_cv2(data, "bgr8")
+        IMG_RES = (data.height, data.width)
     except CvBridgeError as e:
         print(e)
 
@@ -84,6 +93,7 @@ def image_depth_callback(data):
 
     try:
         img_depth_stream = cv_bridge.imgmsg_to_cv2(data, "16UC1")
+        DEPTH_RES = (data.height, data.width)
     except CvBridgeError as e:
         print(e)
 
@@ -363,7 +373,7 @@ if __name__=="__main__":
     rospy.Service("~enable", std_srvs.srv.SetBool, enable_callback)
     rospy.Service("~command", segment_sign_command, command_callback)
 
-    update_camera_params()
+    # update_camera_params()
 
     # visualization in soft sleep, until awake service called
     while not rospy.is_shutdown():
@@ -372,6 +382,7 @@ if __name__=="__main__":
             accum_thresh = rospy.get_param("sign_detector/canny_param2") #30
 
             img_processed = copy(img_rgb_stream)
+            rospy.loginfo("img_processed: {}".format(img_processed))
             keypoints = do_hough_circle_detection(img_processed, copy(img_depth_stream), VISUALIZE=True)
 
             if toggle_patch_visualization: