From 50432d517386569e22557b91285fe0d88293f1b9 Mon Sep 17 00:00:00 2001 From: "Lin Tan(Barry)" Date: Thu, 14 Nov 2024 19:23:35 -0600 Subject: [PATCH] Add files via upload --- autonav_ws/src/autonav_vision/CMakeLists.txt | 26 ++ autonav_ws/src/autonav_vision/LICENSE | 17 ++ autonav_ws/src/autonav_vision/package.xml | 18 ++ .../src/autonav_vision/src/Transformer.py | 231 ++++++++++++++++++ autonav_ws/src/autonav_vision/src/main.py | 32 +++ 5 files changed, 324 insertions(+) create mode 100644 autonav_ws/src/autonav_vision/CMakeLists.txt create mode 100644 autonav_ws/src/autonav_vision/LICENSE create mode 100644 autonav_ws/src/autonav_vision/package.xml create mode 100644 autonav_ws/src/autonav_vision/src/Transformer.py create mode 100644 autonav_ws/src/autonav_vision/src/main.py diff --git a/autonav_ws/src/autonav_vision/CMakeLists.txt b/autonav_ws/src/autonav_vision/CMakeLists.txt new file mode 100644 index 0000000..f135490 --- /dev/null +++ b/autonav_ws/src/autonav_vision/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(autonav_vision) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/autonav_ws/src/autonav_vision/LICENSE b/autonav_ws/src/autonav_vision/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/autonav_ws/src/autonav_vision/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/autonav_ws/src/autonav_vision/package.xml b/autonav_ws/src/autonav_vision/package.xml new file mode 100644 index 0000000..85b8c0a --- /dev/null +++ b/autonav_ws/src/autonav_vision/package.xml @@ -0,0 +1,18 @@ + + + + autonav_vision + 1.0.0 + autonav_vision + lintan + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/autonav_ws/src/autonav_vision/src/Transformer.py b/autonav_ws/src/autonav_vision/src/Transformer.py new file mode 100644 index 0000000..7fcb087 --- /dev/null +++ b/autonav_ws/src/autonav_vision/src/Transformer.py @@ -0,0 +1,231 @@ +import cv2 as cv +import numpy as np +import rclpy +from types import SimpleNamespace +from cv_bridge import CvBridge +from nav_msgs.msg import MapMetaData, OccupancyGrid +import rclpy.qos +from sensor_msgs.msg import CompressedImage +from geometry_msgs.msg import Pose +from cv_bridge import CvBridge + +import json + + + + +g_bridge = CvBridge() + +g_mapData = MapMetaData() +g_mapData.width = 100 +g_mapData.height = 100 +g_mapData.resolution = 0.1 +g_mapData.origin = Pose() +g_mapData.origin.position.x = -10.0 +g_mapData.origin.position.y = -10.0 +#### Need to modify ####################################################### +class FrameTransformerConfig: + + def __init__(self): + + # HSV lower and upper bound value + self.lower_hue = 0 + self.upper_hue = 180 + self.lower_sat = 0 + self.upper_sat = 255 + self.lower_val = 0 + self.upper_val = 255 + + # blur filter + self.blur_size = 5 + self.blur_iterations = 3 + self.map_res = 80 + + # Perspective transform + pts1 = np.float32([[80, 200], [400, 220], [480, 640], [0, 640]]) + pts2 = np.float32([[80, 220], [400, 220], [480, 640], [0, 640]]) + + + # Disabling + self.disable_blur = False + self.disable_hsv = False + self.disable_perspective_transform = False + +########################################################################### + +class FrameTransformer(Node): + + def __init__(self, dir = "left"): + super().__init__("autonav_vision_transformer") + self.config = self.get_default_config() + self.dir = dir + + def init(self): + 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) + self.grid_publisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/raw"), 1) + self.grid_image_publisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image") + "_small", self.qos_profile) + + self.set_device_state(DeviceStateEnum.OPERATING) + + # ways to do the auto hsv calibration + # (1) auto hsv color picking + # (2) Otsu's ? + def onCalibrate(self, msg: CameraCalibration): + + def config_updated(self, jsonobject): + self.config = json.loads(self.jdump(jsonobject), object_hook = lambda d: SimpleNamespace(**d)) + + def get_default_config(self): + return FrameTransformerConfig + + # set up the Gaussian blur kernel size + def get_blur_level(self): + blur_size = self.config.blur_size + return (blur_size, blur_size) + + # get the order of points for image transform + def order_points(self, pts): + # first create a list of points that form a rectangle + rect = np.zeros((4,2), dtype = "float32") + s = pts.sum(axis = 1) + rect[0] = pts[np.argmin(s)] + rect[2] = pts[np.argmin(s)] + + diff = np.diff(pts, axis = 1) + rect[1] = pts[np.argmin(diff)] + rect[2] = pts[np.argmin(diff)] + + return rect + + # Compute the width and height of the new image/frame + def compute_max_width_height(self, rect): + (tl, tr, br, bl) = rect + widthA = np.linalg.norm(br - bl) + widthB = np.linalg.norm(tr - tl) + maxWidth = max(int(widthA), int(widthB)) + + heightA = np.linalg.norm(tr - br) + heightB = np.linalg.norm(tl - bl) + maxheight = max(int(heightA), int(heightB)) + + return maxWidth, maxheight + + # Hough Transform(detect any shape in the image/frame, if the shape can be expressed in a math form) + def hough_transform(self, pts): + + return + + + # Four point transform for bird view of the frame + def four_point_transform(self, image, pts): + if pts.shape != (4, 2): + raise ValueError("Input Points should be a 4x2 array representing four points.") + + rect = self.order_points(pts) + maxWidth, maxHeight = self.compute_max_width_height(rect) + + # Define the destination points for the perspective transformation + dst = np.array([ + [0,0], + [maxWidth - 1, 0], + [maxWidth - 1, maxHeight - 1], + [0, maxHeight - 1]], dtype = "float32") + + # Compute the perspective transform matrix + M = cv.getPerspectiveTransform(rect, dst) + warped = cv.warpPerspective(image, M, (maxWidth, maxHeight)) + + return warped + + # Blurred the image to reduced the noise before convert it into hsv colorspaces to prevent external environments interfere + def blur(self, img): + if self.config.disable_blur: + return img + for _ in range(self.config.blur_iterations): + img = cv.GaussianBlur(img, self.get_blur_level, 0) + + return img + + # HSV color space color-picking + def hsvcolorpicked(): + + return + + # Define the max ROI, should return the bounding box size + def roi_max(self, img): + img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + ret, thresh = cv.threshold(img_gray, 127, 255, 0) + + + + return + + # Define the reference ROI, should return the bounding box size + def roi_reference(): + + return + + # Try to random pick roi within the roi_max + def random_roi(): + + return + + # Compare the similarity of colors(with picked hsv color) with detected objects' + def hsv_calculation(): + + return + + # ROI merge operation to get the shape or contour of detected object more accurately + def roi_merge(): + + return + + + + # Convert the Colorspace + # For the detected object, modify the hsv value until it becomes white + # return the value to config the upper and lower hsv values + # Object in the detected image with white color will be desired. + def get_limits(color): + c = np.uint8([[color]]) + hsvC = cv.cvtColor(c, cv.COLOR_BGR2HSV) + + lower_limits = hsvC[0][0][0] - 10, 100, 100 + upper_limits = hsvC[0][0][0] + 10, 255, 255 + + lower_limits = np.array(lower_limits, dtype = np.uint8) + upper_limits = np.array(upper_limits, dtype = np.uint8) + + return lower_limits, upper_limits + # Pixel rejection(2D convolution and filtering, maybe smoothing?) + + # Receive the Image from the camera(front, back, left, right) + def ImageReceived(self, image: CompressedImage): + img = g_bridge.compressed_imgmsg_to_cv2(image) + self.publish_debug_image(img) + + # blur it + img = self.blur(img) + # Apply filter and return a mask + + +def main(): + rclpy.init() + node_left = ImageTransformer(dir = "left") + node_right = ImageTransformer(dir = "right") + Node.run_nodes([node_left, node_right]) + rclpy.shutdown() + + +if __name__ == "__main__": + main() + + + + + + + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_vision/src/main.py b/autonav_ws/src/autonav_vision/src/main.py new file mode 100644 index 0000000..fae0aee --- /dev/null +++ b/autonav_ws/src/autonav_vision/src/main.py @@ -0,0 +1,32 @@ +## This is the main file to run the Transformer for testing +import cv2 as cv +from PIL import Image +from Transformation import get_limits + + +cap = cv.VideoCapture(0) +yellow = [0, 255, 255] + + +while True: + ret, frame = cap.read() + hsvImage = cv.cvtColor(frame, cv.COLOR_BGR2HSV) + + lowerlimit, upperlimit = get_limits(color = color) + mask = cv.inRange(hsvImage, lowerlimit, upperlimit) + + # Detect the object with the box + mask_temp = Image.fromarray(mask) + bbox = mask_temp.getbbox() + if bbox is not None: + x1, y1, x2, y2 = bbox + frame = cv.rectangle(frame, (x1,y1), (x2,y2), (0, 255, 0), 5) + print(bbox) + + cv.imshow('frame', frame) + + if cv.waitKey(1) & 0xFF == ord('q'): + break + +cap.release() +cv.destroyAllWindows() \ No newline at end of file