From c4b053572532a85bc1bcef21f18b5283b77eadf0 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 2 Jun 2024 20:47:10 -0500 Subject: [PATCH] added the actual setting of the upper/lower thresholds functionality theoretically, no idea if it works, probably doesn't --- .../src/autonav_vision/src/transformations.py | 84 +++++++++++++------ 1 file changed, 57 insertions(+), 27 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 5794d38..2736a34 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -15,6 +15,7 @@ from scr.node import Node from scr.states import DeviceStateEnum +from scr.utils import clamp g_bridge = CvBridge() @@ -26,6 +27,21 @@ g_mapData.origin.position.x = -10.0 g_mapData.origin.position.y = -10.0 +# start in top left, go clockwise +CALIBRATION_BOX = { + "right":[ + (0, 215), + (80, 215), + (80, 290), + (0, 290) + ], + "left":[ + (0, 215), + (400, 215), + (400, 290), + (0, 290) + ] +} class ImageTransformerConfig: def __init__(self): @@ -74,13 +90,13 @@ def __init__(self, dir = "left"): self.config = self.get_default_config() self.dir = dir + # 30 is a good +/- to use when dealing with OpenCV color values + self.pixelFactor = 30 + def directionify(self, topic): return topic + "/" + self.dir def init(self): - # don't break anything - self.system_state = OFF - self.camera_subscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) self.calibration_subscriber = self.create_subscription(CameraCalibration, "/camera/autonav/calibration", self.onCalibrate) self.camera_debug_publisher = self.create_publisher(CompressedImage, self.directionify("/autonav/camera/compressed") + "/cutout", self.qos_profile) @@ -89,33 +105,47 @@ def init(self): self.set_device_state(DeviceStateEnum.OPERATING) - def system_state_transition(self, old: SystemState, updated: SystemState): - self.system_state = updated - def onCalibrate(self, msg: CameraCalibration): # only allow calibration if it's safe to do so, don't want to accidentally ruin a run or kill a person - if self.system_state.state is not AUTONOMOUS and self.system_state.mobility is False: + if self.system_state is not SystemStateEnum.AUTONOMOUS and not self.mobility: # remember that the mask is reversed though, we filter OUT the ground # so ground needs to be in the threshold range, but not obstacles - # include in the mask, so it gets filtered out - if msg.include_in_mask: - pixels = [] #TODO - avgHue, avgSat, avgVal = getAverageVal(pixels) #TODO - - # take the lower of the current and calibrated values, so that as much as possible is included in the mask - self.config.lower_hue = min(self.config.lower_hue, avgHue) - self.config.lower_sat = min(self.config.lower_sat, avgSat) - self.config.lower_val = min(self.config.lower_val, avgVal) - - # take the upper of the current and calibrated values, so that as much as possible is included in the mask - self.config.upper_hue = max(self.config.upper_hue, avgHue) - self.config.upper_sat = max(self.config.upper_sat, avgSat) - self.config.upper_val = max(self.config.upper_val, avgVal) - - # exclude from the mask, so it shows up for expandification - else: - pass #TODO - + pts = CALIBRATION_BOX[self.dir] + + avgH = avgV = avgS = 0 + # for every pixel in the calibration box square thing + for pixel in self.image[pts[0][0]:pts[1][0], pts[0][1]:pts[3][1]]: + avgH += pixel[0] + avgS += pixel[1] + avgV +=pixel[2] + + # then calculate number of pixles and divide out to get the average values + numPixels = abs((pts[0][0] - pts[1][0]) * (pts[0][1] - pts[3][1])) + avgH /= numPixels + avgS /= numPixels + avgV /= numPixels + + # take the lower of the current and calibrated values, so that as much as possible is included in the mask if we are including in the mask, + # else we take the upper of the two to shrink the range of values we filter + # then we have a fudge factor to not overfit or whatever + self.config.lower_hue = min(self.config.lower_hue, avgHue - self.pixelFactor) if msg.include_in_mask else max(self.config.lower_hue, avgHue + self.pixelFactor) + self.config.lower_sat = min(self.config.lower_sat, avgSat - self.pixelFactor) if msg.include_in_mask else max(self.config.lower_sat, avgSat + self.pixelFactor) + self.config.lower_val = min(self.config.lower_val, avgVal - self.pixelFactor) if msg.include_in_mask else max(self.config.lower_val, avgVal + self.pixelFactor) + + # take the upper of the current and calibrated values, so that as much as possible is included in the mask + # else we take the lower of the two values to shrink the range down + self.config.upper_hue = max(self.config.upper_hue, avgHue + self.pixelFactor) if msg.include_in_mask else min(self.config.lower_hue, avgHue - self.pixelFactor) + self.config.upper_sat = max(self.config.upper_sat, avgSat + self.pixelFactor) if msg.include_in_mask else min(self.config.lower_sat, avgSat - self.pixelFactor) + self.config.upper_val = max(self.config.upper_val, avgVal + self.pixelFactor) if msg.include_in_mask else min(self.config.lower_val, avgVal - self.pixelFactor) + + # and we just did some shenanigans with the values and pixelFactor and everything so clamp everything to be safe + self.config.lower_hue = clamp(self.config.lower_hue, 0, 255) + self.config.lower_sat = clamp(self.config.lower_sat, 0, 255) + self.config.lower_val = clamp(self.config.lower_val, 0, 255) + self.config.upper_hue = clamp(self.config.upper_hue, 0, 255) + self.config.upper_sat = clamp(self.config.upper_sat, 0, 255) + self.config.upper_val = clamp(self.config.upper_val, 0, 255) + def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) @@ -246,7 +276,7 @@ def publish_debug_image(self, img): cv2.polylines(img_copy, [np.array(pts)], True, (0, 0, 255), 2) # Draw calibration square points - pts = [] #TODO + pts = CALIBRATION_BOX[self.dir] cv2.polylines(img_copy, [np.array(pts)], True, (255, 0, 0), 2) self.camera_debug_publisher.publish(g_bridge.cv2_to_compressed_imgmsg(img_copy))