putting together path planner

.vscode/settings.json

@@ -0,0 +1,7 @@
+{
+    "python.testing.pytestArgs": [
+        "."
+    ],
+    "python.testing.unittestEnabled": false,
+    "python.testing.pytestEnabled": true
+}

image_processing.py

@@ -3,55 +3,71 @@
 import os
 
 # folder_dir = "pics/"
-badfloor = "pics/"
-
-for images in sorted(os.listdir(badfloor)):
-    image_RGB = cv.imread(badfloor + images)
-    # cv.imshow("read_image", image_RGB)
-    image_HSV = cv.cvtColor(image_RGB, cv.COLOR_BGR2HSV)
-    #
-    # Channel 0 and 1 could be useful if the current method doesn't work.
-    # RGB_channel_0 = cv.extractChannel(image_RGB, 0)
-    # RGB_thresh_2 = cv.threshold(RGB_channel_0, 20, 255, cv.THRESH_BINARY_INV)[1]
-    # cv.imshow("RGB_thresl_0", RGB_thresh_2)
-    # cv.imshow("RGB_channel_0",RGB_channel_0)
-
-    # RGB_channel_1 = cv.extractChannel(image_RGB, 1)
-    # RGB_thresh_2 = cv.threshold(RGB_channel_1, 50, 255, cv.THRESH_BINARY_INV)[1]
-    # cv.imshow("RGB_thresl_1", RGB_thresh_2)
-    # cv.imshow("RGB_channel_1", RGB_channel_1)
-
-    RGB_channel_2 = cv.extractChannel(image_RGB, 2)
+# badfloor = "pics/"
+
+# for images in sorted(os.listdir(badfloor)):
+#     image_RGB = cv.imread(badfloor + images)
+#     # cv.imshow("read_image", image_RGB)
+#     image_HSV = cv.cvtColor(image_RGB, cv.COLOR_BGR2HSV)
+#     #
+#     # Channel 0 and 1 could be useful if the current method doesn't work.
+#     # RGB_channel_0 = cv.extractChannel(image_RGB, 0)
+#     # RGB_thresh_2 = cv.threshold(RGB_channel_0, 20, 255, cv.THRESH_BINARY_INV)[1]
+#     # cv.imshow("RGB_thresl_0", RGB_thresh_2)
+#     # cv.imshow("RGB_channel_0",RGB_channel_0)
+
+#     # RGB_channel_1 = cv.extractChannel(image_RGB, 1)
+#     # RGB_thresh_2 = cv.threshold(RGB_channel_1, 50, 255, cv.THRESH_BINARY_INV)[1]
+#     # cv.imshow("RGB_thresl_1", RGB_thresh_2)
+#     # cv.imshow("RGB_channel_1", RGB_channel_1)
+
+#     RGB_channel_2 = cv.extractChannel(image_RGB, 2)
+#     RGB_thresh_2 = cv.threshold(RGB_channel_2, 35, 255, cv.THRESH_BINARY_INV)[1]
+#     RGB_thresh_22 = cv.threshold(RGB_channel_2, 125, 255, cv.THRESH_BINARY)[1]
+#     mask_RGB1 = cv.erode(RGB_thresh_2, None, iterations=1)
+#     mask_RGB1 = cv.dilate(mask_RGB1, None, iterations=4)
+#     cv.imshow("RGB_channel_2", RGB_channel_2)
+#     cv.imshow("RGB_thresh_2", mask_RGB1)
+#     cv.imshow("RGB_thresh_22", RGB_thresh_22)
+
+#     # This image doesn't seem readily usable for usable image information
+#     # HSV_channel_0 = cv.extractChannel(image_HSV, 0)
+#     # cv.imshow("HSV_channel_0", HSV_channel_0)
+
+#     HSV_channel_1 = cv.extractChannel(image_HSV, 1)
+#     # This threshold can be tuned in the future, but seems to work pretty well for the sample images
+#     threshold_HSV1 = cv.threshold(HSV_channel_1, 120, 255, cv.THRESH_BINARY)[1]
+#     mask_HSV1 = cv.erode(threshold_HSV1, None, iterations=1)
+#     mask_HSV1 = cv.dilate(mask_HSV1, None, iterations=5)
+#     cv.imshow("HSV_channel_1", HSV_channel_1)
+#     # cv.imshow("HSV_channel_1_thres", threshold_HSV1)
+#     cv.imshow("HSV_channel_1_mask", mask_HSV1)
+
+#     # Seems less reliable than other options
+#     # HSV_channel_2 = cv.extractChannel(image_HSV, 2)
+#     # threshold_HSV1 = cv.threshold(HSV_channel_2, 30, 255, cv.THRESH_BINARY_INV)[1]
+#     # cv.imshow("HSV_channel_2_thres", threshold_HSV1)
+#     # cv.imshow("HSV_channel_2", HSV_channel_2)
+
+#     mask_RGB1 = cv.bitwise_or(mask_RGB1, RGB_thresh_22)
+#     obstacles = cv.bitwise_and(mask_HSV1, mask_RGB1)
+#     cv.imshow("obstacles", obstacles)
+#     cv.waitKey(0)
+
+#     cv.imwrite("obstacles_" + images, obstacles)
+
+
+def get_obstacle(rgbimg):
+    image_hsv = cv.cvtColor(rgbimg, cv.COLOR_BGR2HSV)
+    RGB_channel_2 = cv.extractChannel(rgbimg, 2)
     RGB_thresh_2 = cv.threshold(RGB_channel_2, 35, 255, cv.THRESH_BINARY_INV)[1]
     RGB_thresh_22 = cv.threshold(RGB_channel_2, 125, 255, cv.THRESH_BINARY)[1]
     mask_RGB1 = cv.erode(RGB_thresh_2, None, iterations=1)
     mask_RGB1 = cv.dilate(mask_RGB1, None, iterations=4)
-    cv.imshow("RGB_channel_2", RGB_channel_2)
-    cv.imshow("RGB_thresh_2", mask_RGB1)
-    cv.imshow("RGB_thresh_22", RGB_thresh_22)
-
-    # This image doesn't seem readily usable for usable image information
-    # HSV_channel_0 = cv.extractChannel(image_HSV, 0)
-    # cv.imshow("HSV_channel_0", HSV_channel_0)
-
-    HSV_channel_1 = cv.extractChannel(image_HSV, 1)
-    # This threshold can be tuned in the future, but seems to work pretty well for the sample images
+    HSV_channel_1 = cv.extractChannel(image_hsv, 1)
     threshold_HSV1 = cv.threshold(HSV_channel_1, 120, 255, cv.THRESH_BINARY)[1]
     mask_HSV1 = cv.erode(threshold_HSV1, None, iterations=1)
     mask_HSV1 = cv.dilate(mask_HSV1, None, iterations=5)
-    cv.imshow("HSV_channel_1", HSV_channel_1)
-    # cv.imshow("HSV_channel_1_thres", threshold_HSV1)
-    cv.imshow("HSV_channel_1_mask", mask_HSV1)
-
-    # Seems less reliable than other options
-    # HSV_channel_2 = cv.extractChannel(image_HSV, 2)
-    # threshold_HSV1 = cv.threshold(HSV_channel_2, 30, 255, cv.THRESH_BINARY_INV)[1]
-    # cv.imshow("HSV_channel_2_thres", threshold_HSV1)
-    # cv.imshow("HSV_channel_2", HSV_channel_2)
-
     mask_RGB1 = cv.bitwise_or(mask_RGB1, RGB_thresh_22)
     obstacles = cv.bitwise_and(mask_HSV1, mask_RGB1)
-    cv.imshow("obstacles", obstacles)
-    cv.waitKey(0)
-
-    cv.imwrite("obstacles_" + images, obstacles)
+    return obstacles

pathplanner.py

@@ -0,0 +1,18 @@
+from pic2grid import crop_down, make_grid, grid2midpoints
+
+import cv2 as cv
+import numpy as np
+
+
+def find_ave_angle(midpoints):
+    angles = []
+    for i in range(5):
+        curr_pt = midpoints[-1 - i]
+        next_pt = midpoints[-2 - i]
+        dely = next_pt[0] - curr_pt[0]
+        delx = next_pt[1] - curr_pt[1]
+        angle = np.arctan2(delx, -dely)
+        angles.append(angle * (5 - i))
+
+    sum = np.sum(angles)
+    return np.rad2deg(sum / 15.0)

pic2grid.py

@@ -16,6 +16,10 @@ def crop_down(image, crop_height):
     return image[crop_height:, :, :]
 
 
+def crop_up(image, crop_height):
+    return image[:crop_height, :, :]
+
+
 # cropped = crop_down(img, 120)
 # cv.imshow("Cropped Image", cropped)
 # cv.waitKey(0)
@@ -50,7 +54,7 @@ def make_grid(image, n_rows, n_cols, p_occup):
 # cv.waitKey(0)
 
 
-def grid2midpoints(grid):
+def grid2midpoints(grid, scalex, scaley):
     midpoints = []
     for i in range(grid.shape[0]):  # rows
         jl = grid.shape[1] // 2 - 1
@@ -63,7 +67,9 @@ def grid2midpoints(grid):
             jr += 1
         templ = float(jl)
         tempr = float(jr)
-        midpoints.append((i, (templ + tempr) / 2))
+        y = i * scaley
+        x = (templ + tempr) / 2 * scalex
+        midpoints.append((y, x))
     return midpoints
 
 
@@ -72,7 +78,7 @@ def grid2midpoints(grid):
 #     cv.circle(
 #         grid_big,
 #         (
-#             int(midpoints[i][1] * (grid_big.shape[1] // 10)),
+#            #this is bad now int(midpoints[i][1] * (grid_big.shape[1] // 10)),
 #             int(midpoints[i][0] * (grid_big.shape[0] // 10)),
 #         ),
 #         5,

test_pathandgrid.py

@@ -0,0 +1,16 @@
+from pathplanner import find_ave_angle
+from pic2grid import crop_down, make_grid, grid2midpoints
+
+import cv2 as cv
+import numpy as np
+import os
+
+
+def test_find_ave_angle():
+    obstacles = cv.imread("obstacles_image0.jpg")
+    cropped = crop_down(obstacles, 120)
+    grid = make_grid(cropped, 10, 10, 0.33)
+    midpoints = grid2midpoints(grid, cropped.shape[1] // 10, cropped.shape[0] // 10)
+    angle = find_ave_angle(midpoints)
+    print(angle)
+    assert angle == 0.0