migrated content from pref repo

minibot_msgs/CMakeLists.txt

@@ -56,6 +56,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_service_files(
   FILES
   set_url.srv
+  segment_sign_command.srv
 )
 
 ## Generate actions in the 'action' folder

minibot_msgs/srv/segment_sign_command.srv

+int8 PERSISTENT_SAVE=0
+int8 TOGGLE_PATCH_VISUALIZATION=1
+int8 LOAD_DEFAULT=2
+
+int8 command
+---

minibot_vision/CMakeLists.txt

@@ -159,10 +159,11 @@ include_directories(
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+catkin_install_python(PROGRAMS
+  scripts/SegmentSign.py
+  scripts/AdjustSegmentSign.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html

minibot_vision/README.md

@@ -10,8 +10,21 @@ This node detects round objects (like traffic signs) in the rgb image of the rea
 The z value of the pose message is then the detected depth in the camera frame (x and y are not updated).
 The detected signs are cropped to patches and zoomed so that the patch width/height matches our tensorflow requirements.
 This segmentation depends on a some hyper parameters.
-Since it seems to be very robust with the current configuration these are set static in the python script.
-If you need to adjust them there are two different visualization modes for debugging implemented.
+Those are loaded based on the input of `config/sign_detector.yaml`.
+A tool to adjust them can be started by executing `ROS_NAMESPACE=/$ROBOT/; rosrun minibot_vision SegmentSign.py` and works with following hotkeys: 
+- p: Toggle patch visualization [default: True]
+- d: Load default parameters (specified within `config/sign_detector_default.yaml`). This will neither override `config/sign_detector.yaml` nor the ros-params.
+- s: Save the current parameters persistent to `config/sign_detector.yaml`. This will also upload to ros-param.
+
+## Guideline for optimal Sign Detection
+The hough transform used to pre-detect the signs works best if the actual traffic signs have the following conditions:
+- non reflecting signs (eg. paper)
+- signs should be surrounded by a clear circle that is at best not connected to the content within the sign (helps hough transform)
+- there should be some, at best indirect, light around the signs. The more indirect light the better the detection usually
+
+If there are still issues with the segmentation/detection check these points:
+- if the hough detection is bad, try to adjust the two hough parameters (see section **Segment Sign**)
+- the background of the signs is very important during classification
 
 ## Sign Detector
 This node starts the SegmentSign node together with a tensorflow classification.

minibot_vision/config/sign_detector.yaml

 sign_detector:
+  canny_param1: 144
+  canny_param2: 59
   img_height: 480
   img_width: 640
-  canny_param1: 100
-  canny_param2: 100 # good light: 40; bad light: 100
-  min_depth: 0.2
   max_depth: 1.0
-  visualize: False
-  zoom_threshold: 1.15
+  max_radius: 128
+  min_depth: 0.2
   min_radius: 15
-  max_radius: 128
+  visualize: false
\ No newline at end of file
+  zoom_threshold: 1.15

minibot_vision/config/sign_detector_default.yaml

+# default params that got not overriden
+sign_detector:
+  img_height: 480
+  img_width: 640
+  canny_param1: 100
+  canny_param2: 100 # good light: 40; bad light: 100
+  min_depth: 0.2
+  max_depth: 1.0
+  visualize: False
+  zoom_threshold: 1.15
+  min_radius: 15
+  max_radius: 128

minibot_vision/scripts/AdjustSegmentSign.py

+#!/usr/bin/env python3
+
+"""
+This serves as a tool to adjust the hyperparameters used in SegmentSign.py
+"""
+
+import rospy
+import cv2
+from sensor_msgs.msg import CompressedImage
+import std_srvs.srv
+from minibot_msgs.srv import segment_sign_command, segment_sign_commandRequest, segment_sign_commandResponse
+from cv_bridge import CvBridge, CvBridgeError
+import numpy as np
+
+# *** GLOBALS ***
+bridge = CvBridge()
+img_rgb_stream = np.zeros((600, 800))
+canny = -1
+accum_thresh = -1
+
+
+def update_params_canny(d):
+    global canny
+
+    # upload new param to param server
+    rospy.set_param("/minibot/sign_detector/canny_param1", cv2.getTrackbarPos("Canny", "image"))
+
+
+def update_params_accum_thresh(d):
+    global accum_thresh
+
+    # upload new param to param server
+    rospy.set_param("/minibot/sign_detector/canny_param2", cv2.getTrackbarPos("Accum", "image") )
+
+
+def compressed_image_callback(ros_data):
+    global bridge, img_rgb_stream
+
+    try:
+        np_arr = np.fromstring(ros_data.data, np.uint8)
+        img_rgb_stream = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+    except CvBridgeError as e:
+        print(e)
+
+
+def call_enable(is_enable):
+    service = "/minibot/hough_transform/enable"
+
+    #msg = std_srvs.srv.SetBool()
+    #msg.data = is_enable
+
+    try:
+        call_place = rospy.ServiceProxy(service, std_srvs.srv.SetBool())
+        response = call_place.call(is_enable)
+    except rospy.ServiceException as e:
+        print("Service call failed: %s" % e)
+
+
+def call_segment_sign_command(command):
+    # TODO make the namespace as parameter
+    service = "/minibot/hough_transform/command"
+
+    if command == 0:
+        command_msg = segment_sign_commandRequest(command=segment_sign_commandRequest.PERSISTENT_SAVE)
+    elif command == 1:
+        command_msg = segment_sign_commandRequest(command=segment_sign_commandRequest.TOGGLE_PATCH_VISUALIZATION)
+    elif command == 2:
+        command_msg = segment_sign_commandRequest(command=segment_sign_commandRequest.LOAD_DEFAULT)
+
+    try:
+        call_place = rospy.ServiceProxy(service, segment_sign_command)
+        response = call_place.call(command_msg)
+    except rospy.ServiceException as e:
+        print("Service call failed: %s" % e)
+
+
+if __name__=="__main__":
+    rospy.init_node("adjust_segment_sign")
+
+    # get hyperparams
+    canny = rospy.get_param("/minibot/sign_detector/canny_param1")
+    accum_thresh = rospy.get_param("/minibot/sign_detector/canny_param2")
+
+    # init cv2
+    cv2.namedWindow("image")
+    cv2.resizeWindow("image", 800, 600)
+    cv2.createTrackbar("Canny", "image", canny, 255, update_params_canny)
+    cv2.setTrackbarMin("Canny", "image", 1)
+    cv2.createTrackbar("Accum", "image", accum_thresh, 255, update_params_accum_thresh)
+    cv2.setTrackbarMin("Accum", "image", 1)
+
+    rospy.Subscriber("/minibot/hough_transform/result_image/compressed", CompressedImage, compressed_image_callback)
+
+    # enable visualization
+    call_enable(True)
+
+    rate = rospy.Rate(30)
+    while not rospy.is_shutdown():
+        cv2.imshow("image", img_rgb_stream)
+
+        canny = rospy.get_param("/minibot/sign_detector/canny_param1")  #100
+        accum_thresh = rospy.get_param("/minibot/sign_detector/canny_param2") #30
+        cv2.setTrackbarPos("Canny", "image", canny)
+        cv2.setTrackbarPos("Accum", "image", accum_thresh)
+
+        k = cv2.waitKey(1) & 0xFF
+        if k == ord("p"):
+            call_segment_sign_command(1)
+        if k == ord("d"):
+            call_segment_sign_command(2)
+        if k == ord("s"):
+            call_segment_sign_command(0)
+        if k == ord("q"):
+            break
+
+        rate.sleep()
+
+    # disable visualization
+    call_enable(False)
+
+    cv2.destroyAllWindows()
+

minibot_vision/scripts/SegmentSign.py

+#!/usr/bin/env python3
+
 import cv2
 import numpy as np
 from cv_bridge import CvBridge, CvBridgeError
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import Image, CompressedImage
 import rospy
 from copy import copy
+import yaml
+import rospkg
+import std_srvs.srv
+from minibot_msgs.srv import segment_sign_command, segment_sign_commandRequest, segment_sign_commandResponse
 
 # *** hyper params ***
 #IMG_RES = (480, 640)
@@ -25,16 +31,15 @@ MAX_RADIUS = rospy.get_param("sign_detector/max_radius", 128)  # 128
 cv_bridge = CvBridge()
 img_rgb_stream = np.zeros((IMG_RES[0], IMG_RES[1], 3), np.uint8)
 img_depth_stream = np.zeros((IMG_RES[0], IMG_RES[1], 1), np.uint8)
+ros_pack = None
+segment_enable = False
+segment_rate = None
+toggle_patch_visualization = True
+bridge = CvBridge()
 
 def empty(d):
     pass
 
-if VISUALIZE:
-    cv2.namedWindow("Parameters")
-    cv2.resizeWindow("Parameters", 800, 600)
-    cv2.createTrackbar("Canny", "Parameters", canny, 255, empty)
-    cv2.createTrackbar("Accum", "Parameters", accum_thresh, 255, empty)
-
 
 def image_color_callback(data):
     global img_rgb_stream, cv_bridge
@@ -75,7 +80,7 @@ def circular_mean(p, r, arr : np.array):
     return sum_px_values / count_px
 
 
-def do_hough_circle_detection(img_rgb, img_depth):
+def do_hough_circle_detection(img_rgb, img_depth, VISUALIZE=False):
     global canny, accum_thresh
 
     gray = img_rgb
@@ -189,38 +194,140 @@ def filter_duplicate_keypoints(keypoints):
     return required_keypoints
 
 
+def save_params():
+    """
+    During VISUALIZE mode.
+    Saves the current configured parameter set permanently and in addition loads them to rosparam.
+    """
+    global canny, accum_thresh, ros_pack
+
+    # write to file
+    path = "{}/config/sign_detector.yaml".format(ros_pack.get_path("minibot_vision"))
+    with open(path, "r") as stream:
+        try:
+            data = yaml.safe_load(stream)
+        except yaml.YAMLError as exc:
+            print(exc)
+            return
+
+    with open(path, "w") as stream:
+        try:
+            data['sign_detector']['canny_param1'] = canny
+            data['sign_detector']['canny_param2'] = accum_thresh
+            yaml.safe_dump(data, stream)
+        except yaml.YAMLError as exc:
+            print(exc)
+            return
+
+    # save to rosparam
+    rospy.set_param("sign_detector/canny_param1", canny)
+    rospy.get_param("sign_detector/canny_param2", accum_thresh)
+
+    rospy.loginfo("({}) Saved new params persistent".format(rospy.get_name()))
+
+def load_default_params():
+    """
+    During VISUALIZE mode.
+    Loads the default params to the ui. These params are neither persistent saved nore loaded to rosparam.
+    """
+    global canny, accum_thresh, ros_pack
+
+    with open("{}/config/sign_detector_default.yaml".format(ros_pack.get_path("minibot_vision"))) as stream:
+        try:
+            data = yaml.safe_load(stream)
+        except yaml.YAMLError as exc:
+            print(exc)
+
+        canny = data['sign_detector']['canny_param1']
+        accum_thresh = data['sign_detector']['canny_param2']
+        #cv2.setTrackbarPos("Canny", "Parameters", canny)
+        #cv2.setTrackbarPos("Accum", "Parameters", accum_thresh)
+        rospy.set_param("sign_detector/canny_param1", canny)
+        rospy.set_param("sign_detector/canny_param2", accum_thresh)
+
+        rospy.loginfo("({}) Loaded default params".format(rospy.get_name()))
+
+
+def enable_callback(req):
+    global segment_enable, segment_rate
+
+    segment_enable = req.data
+    rospy.loginfo("({}) set enable to {}".format(rospy.get_name(), segment_enable))
+    # go in low power mode if the node is doing nothing
+    if segment_enable:
+        segment_rate = rospy.Rate(30)
+    else:
+        segment_rate = rospy.Rate(5)
+
+    return True, ""
+
+
+def command_callback(req):
+    """
+    Callback to remote control the visualization of the segment sign hyper param adjustment.
+    """
+    global toggle_patch_visualization
+
+    if req.command == segment_sign_commandRequest.PERSISTENT_SAVE:
+        rospy.loginfo("({}) save params persistent".format(rospy.get_name()))
+        save_params()
+    elif req.command == segment_sign_commandRequest.TOGGLE_PATCH_VISUALIZATION:
+        rospy.loginfo("({}) Toggle patch visualisation is {}".format(rospy.get_name(), toggle_patch_visualization))
+        toggle_patch_visualization = not toggle_patch_visualization
+    elif req.command == segment_sign_commandRequest.LOAD_DEFAULT:
+        rospy.loginfo("({}) load default params".format(rospy.get_name()))
+        load_default_params()
+    else:
+        rospy.logwarn("({}) command {} not known".format(rospy.get_name(), req.command))
+
+    return segment_sign_commandResponse()
+
+
+def publish_image(img, pub_cmpr_img):
+    global bridge
+
+    # use same timestamp for synchronisation
+    timestamp = rospy.Time.now()
+
+    # publish compressed img for website visualization
+    cmprsmsg = bridge.cv2_to_compressed_imgmsg(img)
+    cmprsmsg.header.stamp = timestamp
+    pub_cmpr_img.publish(cmprsmsg)
+
 
 if __name__=="__main__":
     rospy.init_node("hough_detection")
+    ros_pack = rospkg.RosPack()
+    ns = rospy.get_namespace()
+    segment_rate = rospy.Rate(5)
 
+    # TODO docu
 
-    ns = "/minibot/"#rospy.get_namespace()
-    VISUALIZE = True
+    # *** TOPICS
     img_color_topic = "{}camera/color/image_raw".format(ns)
     img_depth_topic = "{}camera/aligned_depth_to_color/image_raw".format(ns)
     rospy.Subscriber(img_color_topic, Image, image_color_callback)
     rospy.Subscriber(img_depth_topic, Image, image_depth_callback)
+    pub_cmpr_img = rospy.Publisher("~result_image/compressed", CompressedImage, queue_size=10)
+
+    # *** SERVICES
+    rospy.Service("~enable", std_srvs.srv.SetBool, enable_callback)
+    rospy.Service("~command", segment_sign_command, command_callback)
 
-    toggle_patch_visualization = True
-    print("Toggle patch visualisation is {}. Press p to change.".format(toggle_patch_visualization))
-    rate = rospy.Rate(30)
+    # visualization in soft sleep, until awake service called
     while not rospy.is_shutdown():
-        img_processed = copy(img_rgb_stream)
-        keypoints = do_hough_circle_detection(img_processed, copy(img_depth_stream))
+        if segment_enable:
+            canny = rospy.get_param("sign_detector/canny_param1")  #100
+            accum_thresh = rospy.get_param("sign_detector/canny_param2") #30
 
-        if toggle_patch_visualization:
             img_processed = copy(img_rgb_stream)
-            patches = get_tensor_patches(copy(img_rgb_stream), keypoints)
-            visualize_patches(keypoints, patches, ["d:{:1.3f}".format(k["depth"]) for k in keypoints], img_processed)
-
-        cv2.imshow("Parameters", img_processed)
-        canny = cv2.getTrackbarPos("Canny", "Parameters")
-        accum_thresh = cv2.getTrackbarPos("Accum", "Parameters")
+            keypoints = do_hough_circle_detection(img_processed, copy(img_depth_stream), VISUALIZE=True)
 
-        k = cv2.waitKey(1) & 0xFF
-        if k == ord("p"):
-            print("Toggle patch visualisation is {}".format(toggle_patch_visualization))
-            toggle_patch_visualization = not toggle_patch_visualization
+            if toggle_patch_visualization:
+                img_processed = copy(img_rgb_stream)
+                patches = get_tensor_patches(copy(img_rgb_stream), keypoints)
+                img_processed = visualize_patches(keypoints, patches, ["d:{:1.3f}".format(k["depth"]) for k in keypoints], img_processed)
 
+            publish_image(img_processed, pub_cmpr_img)
 
-        rate.sleep()
+        segment_rate.sleep()

minibot_vision/scripts/ShapeDetector.py

@@ -93,7 +93,7 @@ def do_shape_detection(img_rgb, img_depth):
     if VISUALIZE:
         cv2.imshow('thresh img', thresh1)
 
-    contours, hierarchy = cv2.findContours(thresh1, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[-2:]
+    contours, hierarchy = cv2.findContours(thresh1, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
     keypoints = []
     for cnt in contours:
         approx = cv2.approxPolyDP(cnt,0.01*cv2.arcLength(cnt,True),True)
@@ -142,4 +142,4 @@ if __name__=="__main__":
         #cv2.imshow("Shape", img_processed)
 
 
-        rate.sleep()
+        rate.sleep()
\ No newline at end of file