diff --git a/python_wip/Python Unit Tests.py b/actions_scripts/Python Unit Tests.py similarity index 99% rename from python_wip/Python Unit Tests.py rename to actions_scripts/Python Unit Tests.py index 822636b..f647bee 100644 --- a/python_wip/Python Unit Tests.py +++ b/actions_scripts/Python Unit Tests.py @@ -1,5 +1,3 @@ -# my_package/test/test_bbox_node.py - import unittest import numpy as np from my_package.bbox_node import BBoxNode diff --git a/calibration_tools/README.md b/calibration_tools/README.md new file mode 100644 index 0000000..a23c192 --- /dev/null +++ b/calibration_tools/README.md @@ -0,0 +1,3 @@ +# Colorspace calibration + +Use q to quit, a/d for navigation, r to toggle rectanglers, and c to toggle countours \ No newline at end of file diff --git a/calibration_tools/camera_calibration.py b/calibration_tools/camera_calibration.py deleted file mode 100644 index da0a4a2..0000000 --- a/calibration_tools/camera_calibration.py +++ /dev/null @@ -1,258 +0,0 @@ -import pyzed.sl as sl -import matplotlib.pyplot as plt -import numpy as np -import cv2 -import os -import pandas as pd - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -df = pd.read_csv('values.csv') - -default_brightness = df['brightness'][0] -default_contrast = df['contrast'][0] -default_hue = df['hue'][0] -default_saturation = df['saturation'][0] -default_sharpness = df['sharpness'][0] -default_gamma = df['gamma'][0] -default_gain = df['gain'][0] - -def onTrack1(val): - global roi_x - roi_x=val - print('roi x',roi_x) -def onTrack2(val): - global roi_w - roi_w=val - print('roi w',roi_w) -def onTrack3(val): - global roi_y - roi_y=val - print('roi y',roi_y) -def onTrack4(val): - global roi_h - roi_h=val - print('roi h',roi_h) -def onTrack4(val): - global roi_h - roi_h=val - print('roi h',roi_h) - -cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('roi calibration', window_width, window_height) -cv2.moveWindow('roi calibration',0,0) - -cv2.createTrackbar('x pos','roi calibration',1,width-1,onTrack1) -cv2.createTrackbar('width','roi calibration',width,width,onTrack2) -cv2.createTrackbar('y pos','roi calibration',1,height-1,onTrack3) -cv2.createTrackbar('height','roi calibration',height,height,onTrack4) - -init = sl.InitParameters() -cam = sl.Camera() -init.camera_resolution = sl.RESOLUTION.HD1080 -init.camera_fps = 30 - -if not cam.is_opened(): - print("Opening ZED Camera ") -status = cam.open(init) -if status != sl.ERROR_CODE.SUCCESS: - print(repr(status)) - exit() - -runtime = sl.RuntimeParameters() -mat = sl.Mat() - -while True: - err = cam.grab(runtime) - if err == sl.ERROR_CODE.SUCCESS: - cam.retrieve_image(mat, sl.VIEW.LEFT_UNRECTIFIED) - image = mat.get_data() - image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) - - frame = image[roi_y:roi_h+1, roi_x:roi_w+1] - - blank_image = np.zeros((window_height, window_width, 3), np.uint8) - x_offset = max((window_width - frame.shape[1]) // 2, 0) - y_offset = max((window_height - frame.shape[0]) // 2, 0) - - blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame - - cv2.imshow('roi calibration', blank_image) - if cv2.waitKey(1) & 0xff ==ord('q'): - break - -cam.close() -print("ZED Camera closed") -cv2.destroyAllWindows() - -# # Get all connected cameras -# cameras = sl.Camera.get_device_list() -# for cam in cameras: -# # Create and open the camera for each serial number -# zed = self.open_camera(cam.serial_number) - -# old: -import pyzed.sl as sl -import matplotlib.pyplot as plt -import numpy as np -import cv2 -import os -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") -height, width, _ = image.shape -window_height, window_width = height, width -roi_x = 1 -roi_w = width -roi_y = 1 -roi_h = height -def onTrack1(val): - global roi_x - roi_x=val - print('roi x',roi_x) -def onTrack2(val): - global roi_w - roi_w=val - print('roi w',roi_w) -def onTrack3(val): - global roi_y - roi_y=val - print('roi y',roi_y) -def onTrack4(val): - global roi_h - roi_h=val - print('roi h',roi_h) -cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('roi calibration', window_width, window_height) -cv2.moveWindow('roi calibration',0,0) -cv2.createTrackbar('x pos','roi calibration',1,width-1,onTrack1) -cv2.createTrackbar('width','roi calibration',width,width,onTrack2) -cv2.createTrackbar('y pos','roi calibration',1,height-1,onTrack3) -cv2.createTrackbar('height','roi calibration',height,height,onTrack4) - -init = sl.InitParameters() -cam = sl.Camera() -init.camera_resolution = sl.RESOLUTION.HD1080 -init.camera_fps = 30 - -if not cam.is_opened(): - print("Opening ZED Camera ") -status = cam.open(init) -if status != sl.ERROR_CODE.SUCCESS: - print(repr(status)) - exit() - -runtime = sl.RuntimeParameters() -mat = sl.Mat() - -while True: - err = cam.grab(runtime) - if err == sl.ERROR_CODE.SUCCESS: - cam.retrieve_image(mat, sl.VIEW.LEFT_UNRECTIFIED) - image = mat.get_data() - image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) - - frame = image[roi_y:roi_h+1, roi_x:roi_w+1] - - blank_image = np.zeros((window_height, window_width, 3), np.uint8) - x_offset = max((window_width - frame.shape[1]) // 2, 0) - y_offset = max((window_height - frame.shape[0]) // 2, 0) - - blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame - - cv2.imshow('roi calibration', blank_image) - if cv2.waitKey(1) & 0xff ==ord('q'): - break - -cam.close() -print("ZED Camera closed") -cv2.destroyAllWindows() - -# newest -import pyzed.sl as sl -import matplotlib.pyplot as plt -import numpy as np -import cv2 -import os -import pandas as pd - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -height, width, _ = image.shape -window_height, window_width = height, width -df = pd.read_csv('values.csv') - -roi_x = 1 -roi_w = width -roi_y = 1 -roi_h = height -default_brightness = df['brightness'][0] -default_contrast = df['contrast'][0] -default_hue = df['hue'][0] -default_saturation = df['saturation'][0] -default_sharpness = df['sharpness'][0] -default_gamma = df['gamma'][0] -default_gain = df['gain'][0] - -def onTrack1(val): - global roi_x - roi_x=val - print('roi x',roi_x) -def onTrack2(val): - global roi_w - roi_w=val - print('roi w',roi_w) -def onTrack3(val): - global roi_y - roi_y=val - print('roi y',roi_y) -def onTrack4(val): - global roi_h - roi_h=val - print('roi h',roi_h) -def onTrack4(val): - global roi_h - roi_h=val - print('roi h',roi_h) - -cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('roi calibration', window_width, window_height) -cv2.moveWindow('roi calibration',0,0) -cv2.createTrackbar('x pos','roi calibration',1,width-1,onTrack1) -cv2.createTrackbar('width','roi calibration',width,width,onTrack2) -cv2.createTrackbar('y pos','roi calibration',1,height-1,onTrack3) -cv2.createTrackbar('height','roi calibration',height,height,onTrack4) -init = sl.InitParameters() -cam = sl.Camera() -init.camera_resolution = sl.RESOLUTION.HD1080 -init.camera_fps = 30 -if not cam.is_opened(): - print("Opening ZED Camera ") -status = cam.open(init) -if status != sl.ERROR_CODE.SUCCESS: - print(repr(status)) - exit() -runtime = sl.RuntimeParameters() -mat = sl.Mat() -while True: - err = cam.grab(runtime) - if err == sl.ERROR_CODE.SUCCESS: - cam.retrieve_image(mat, sl.VIEW.LEFT_UNRECTIFIED) - image = mat.get_data() - image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) - - frame = image[roi_y:roi_h+1, roi_x:roi_w+1] - - blank_image = np.zeros((window_height, window_width, 3), np.uint8) - x_offset = max((window_width - frame.shape[1]) // 2, 0) - y_offset = max((window_height - frame.shape[0]) // 2, 0) - - blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame - - cv2.imshow('roi calibration', blank_image) - if cv2.waitKey(1) & 0xff ==ord('q'): - break -cam.close() -print("ZED Camera closed") -cv2.destroyAllWindows() \ No newline at end of file diff --git a/calibration_tools/colorspace_calibration.py b/calibration_tools/colorspace_calibration.py index f28aada..28c91f3 100644 --- a/calibration_tools/colorspace_calibration.py +++ b/calibration_tools/colorspace_calibration.py @@ -1,452 +1,131 @@ import matplotlib.pyplot as plt import matplotlib.colors import numpy as np -import pandas as pd import cv2 import os - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -df = pd.read_csv('values.csv') - -default_hue_light = df['hue_light'][0] -default_sat_light = df['sat_light'][0] -default_val_light = df['val_light'][0] -default_hue_dark = df['hue_dark'][0] -default_sat_dark = df['sat_dark'][0] -default_val_dark = df['val_dark'][0] -default_area = df['area'][0] - -height, width, _ = image.shape -window_height, window_width = height, width - -hue_light = 10 -sat_light = 20 -val_light = 10 -hue_dark = 250 -sat_dark = 10 -val_dark = 250 -area = 100 -draw_countours = 1 -draw_rectangles = 1 - -def onTrack1(val): - global hue_light - hue_light=val -def onTrack2(val): - global hue_dark - hue_dark=val -def onTrack3(val): - global sat_light - sat_light=val -def onTrack4(val): - global sat_dark - sat_dark=val -def onTrack5(val): - global val_light - val_light=val -def onTrack6(val): - global val_dark - val_dark=val -def onTrack7(val): - global area - area=val -def onTrack8(val): - global draw_countours - draw_countours=val -def onTrack9(val): - global draw_rectangles - draw_rectangles=val - - -cv2.namedWindow('colorspace calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('colorspace calibration', window_width, window_height) -cv2.moveWindow('colorspace calibration',0,0) - -cv2.createTrackbar('Hue Low','colorspace calibration',default_hue_light,179,onTrack1) -cv2.createTrackbar('Hue High','colorspace calibration',default_hue_dark,179,onTrack2) -cv2.createTrackbar('Sat Low','colorspace calibration',default_sat_light,255,onTrack3) -cv2.createTrackbar('Sat High','colorspace calibration',default_sat_dark,255,onTrack4) -cv2.createTrackbar('Val Low','colorspace calibration',default_val_light,255,onTrack5) -cv2.createTrackbar('Val High','colorspace calibration',default_val_dark,255,onTrack6) -cv2.createTrackbar('Area','colorspace calibration',default_area,window_height*window_width,onTrack7) -cv2.createTrackbar('Contours','colorspace calibration',1,1,onTrack8) -cv2.createTrackbar('Rectangles','colorspace calibration',1,1,onTrack9) - -while True: - frameHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV) - - lowerBound=np.array([hue_light,sat_light,val_light]) - upperBound=np.array([hue_dark,sat_dark,val_dark]) - - lo_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_light,sat_light,val_light], dtype=np.uint8) / 255.0 - do_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_dark,sat_dark,val_dark], dtype=np.uint8) / 255.0 +import argparse + +hue_light = 0 +sat_light = 0 +val_light = 0 +hue_dark = 255 +sat_dark = 255 +val_dark = 255 +default_area = 100 + +def color_calibrator(image_path='C:/Users/ishaa/Coding Projects/Applied-AI/ROS/assets/maize'): + if not os.path.exists(image_path): + raise ValueError(f"Images folder not found at {image_path}") + if len(os.listdir(image_path)) == 0: + raise ValueError(f"Images folder is empty") - lo_square_rgb = matplotlib.colors.hsv_to_rgb(lo_square) - do_square_rgb = matplotlib.colors.hsv_to_rgb(do_square) + index = 0 + files = [] + os.chdir(image_path) + for filename in os.listdir(image_path): + if filename.endswith(".jpg") or filename.endswith(".JPG"): + files.append(image_path + '/' + filename) - lo_square_bgr = cv2.cvtColor((lo_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - do_square_bgr = cv2.cvtColor((do_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - color_square = cv2.vconcat([lo_square_bgr, do_square_bgr]) - - myMask=cv2.inRange(frameHSV,lowerBound,upperBound) - myObject=cv2.bitwise_and(image,image,mask=myMask) - - contours, _ = cv2.findContours(myMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - for cnt in contours: - if draw_countours: - cv2.drawContours(myObject, [cnt], -1, (0, 0, 255), 1) - if cv2.contourArea(cnt) > 1000: - x, y, w, h = cv2.boundingRect(cnt) - if draw_rectangles: - if (w * h) > area and max(w/h , h/w) < 5 and (w * h) < (window_height * window_width / 4): - cv2.rectangle(myObject, (x, y), (x + w, y + h), (0, 255, 0), 2) - - frame = cv2.hconcat([myObject, color_square]) - cv2.imshow('colorspace calibration', frame) - - if cv2.waitKey(1) & 0xff ==ord('q'): - df['hue_light'][0] = hue_light - df['sat_light'][0] = sat_light - df['val_light'][0] = val_light - df['hue_dark'][0] = hue_dark - df['sat_dark'][0] = sat_dark - df['val_dark'][0] = val_dark - df['area'][0] = area - df.to_csv('values.csv', index=False) - break - -cv2.destroyAllWindows() - -# old: -import matplotlib.pyplot as plt -import matplotlib.colors -import numpy as np -import cv2 -import os - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -height, width, _ = image.shape -window_height, window_width = height, width - -hue_light = 10 -sat_light = 20 -val_light = 10 -hue_dark = 250 -sat_dark = 10 -val_dark = 250 -area = 100 - -def onTrack1(val): - global hue_light - hue_light=val - print('Hue Low',hue_light) -def onTrack2(val): - global hue_dark - hue_dark=val - print('Hue High',hue_dark) -def onTrack3(val): - global sat_light - sat_light=val - print('Sat Low',sat_light) -def onTrack4(val): - global sat_dark - sat_dark=val - print('Sat High',sat_dark) -def onTrack5(val): - global val_light - val_light=val - print('Val Low',val_light) -def onTrack6(val): - global val_dark - val_dark=val - print('Val High',val_dark) -def onTrack7(val): - global area - area=val - print('Area',area) - - -cv2.namedWindow('colorspace calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('colorspace calibration', window_width, window_height) -cv2.moveWindow('colorspace calibration',0,0) - -cv2.createTrackbar('Hue Low','colorspace calibration',10,179,onTrack1) -cv2.createTrackbar('Hue High','colorspace calibration',20,179,onTrack2) -cv2.createTrackbar('Sat Low','colorspace calibration',10,255,onTrack3) -cv2.createTrackbar('Sat High','colorspace calibration',250,255,onTrack4) -cv2.createTrackbar('Val Low','colorspace calibration',10,255,onTrack5) -cv2.createTrackbar('Val High','colorspace calibration',250,255,onTrack6) -cv2.createTrackbar('Area','colorspace calibration',100,window_height*window_width,onTrack7) - -while True: - frameHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV) - - lowerBound=np.array([hue_light,sat_light,val_light]) - upperBound=np.array([hue_dark,sat_dark,val_dark]) - - lo_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_light,sat_light,val_light], dtype=np.uint8) / 255.0 - do_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_dark,sat_dark,val_dark], dtype=np.uint8) / 255.0 - lo_square_rgb = matplotlib.colors.hsv_to_rgb(lo_square) - do_square_rgb = matplotlib.colors.hsv_to_rgb(do_square) - lo_square_bgr = cv2.cvtColor((lo_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - do_square_bgr = cv2.cvtColor((do_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - color_square = cv2.vconcat([lo_square_bgr, do_square_bgr]) - - myMask=cv2.inRange(frameHSV,lowerBound,upperBound) - myObject=cv2.bitwise_and(image,image,mask=myMask) - - contours, _ = cv2.findContours(myMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - for cnt in contours: - cv2.drawContours(myObject, [cnt], -1, (0, 0, 255), 1) - if cv2.contourArea(cnt) > 100: - x, y, w, h = cv2.boundingRect(cnt) - if w * h > area and max(w/h , h/w) < 5: - print(w*h) - cv2.rectangle(myObject, (x, y), (x + w, y + h), (0, 255, 0), 1) - - frame = cv2.hconcat([myObject, color_square]) - cv2.imshow('colorspace calibration', frame) - - if cv2.waitKey(1) & 0xff ==ord('q'): - break - -cv2.destroyAllWindows() - -# old pt 3 -import matplotlib.pyplot as plt -import matplotlib.colors -import numpy as np -import pandas as pd -import cv2 -import os -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") -df = pd.read_csv('values.csv') -default_hue_light = df['hue_light'][0] -default_sat_light = df['sat_light'][0] -default_val_light = df['val_light'][0] -default_hue_dark = df['hue_dark'][0] -default_sat_dark = df['sat_dark'][0] -default_val_dark = df['val_dark'][0] -default_area = df['area'][0] -height, width, _ = image.shape -window_height, window_width = height, width -hue_light = 10 -sat_light = 20 -val_light = 10 -hue_dark = 250 -sat_dark = 10 -val_dark = 250 -area = 100 -draw_countours = True -draw_rectangles = True - -def onTrack1(val): - global hue_light - hue_light=val - print('Hue Low',hue_light) -def onTrack2(val): - global hue_dark - hue_dark=val - print('Hue High',hue_dark) -def onTrack3(val): - global sat_light - sat_light=val - print('Sat Low',sat_light) -def onTrack4(val): - global sat_dark - sat_dark=val - print('Sat High',sat_dark) -def onTrack5(val): - global val_light - val_light=val - print('Val Low',val_light) -def onTrack6(val): - global val_dark - val_dark=val - print('Val High',val_dark) -def onTrack7(val): - global area - area=val - print('Area',area) -def toggle_contours(val): - global draw_contours - draw_contours = not draw_contours -def toggle_rectangles(val): - global draw_rectangles - draw_rectangles = not draw_rectangles - - - -cv2.namedWindow('colorspace calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('colorspace calibration', window_width, window_height) -cv2.moveWindow('colorspace calibration',0,0) -cv2.createTrackbar('Hue Low','colorspace calibration',default_hue_light,179,onTrack1) -cv2.createTrackbar('Hue High','colorspace calibration',default_hue_dark,179,onTrack2) -cv2.createTrackbar('Sat Low','colorspace calibration',default_sat_light,255,onTrack3) -cv2.createTrackbar('Sat High','colorspace calibration',default_sat_dark,255,onTrack4) -cv2.createTrackbar('Val Low','colorspace calibration',default_val_light,255,onTrack5) -cv2.createTrackbar('Val High','colorspace calibration',default_val_dark,255,onTrack6) -cv2.createTrackbar('Area','colorspace calibration',default_area,window_height*window_width,onTrack7) -cv2.createButton("Toggle Contours", toggle_contours, None, cv2.QT_PUSH_BUTTON, 1) -cv2.createButton("Toggle Rectangles", toggle_rectangles, None, cv2.QT_PUSH_BUTTON, 1) - -while True: - frameHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV) - - lowerBound=np.array([hue_light,sat_light,val_light]) - upperBound=np.array([hue_dark,sat_dark,val_dark]) - - lo_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_light,sat_light,val_light], dtype=np.uint8) / 255.0 - do_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_dark,sat_dark,val_dark], dtype=np.uint8) / 255.0 - lo_square_rgb = matplotlib.colors.hsv_to_rgb(lo_square) - do_square_rgb = matplotlib.colors.hsv_to_rgb(do_square) - lo_square_bgr = cv2.cvtColor((lo_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - do_square_bgr = cv2.cvtColor((do_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - color_square = cv2.vconcat([lo_square_bgr, do_square_bgr]) - - myMask=cv2.inRange(frameHSV,lowerBound,upperBound) - myObject=cv2.bitwise_and(image,image,mask=myMask) - - contours, _ = cv2.findContours(myMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - for cnt in contours: - if draw_countours: - cv2.drawContours(myObject, [cnt], -1, (0, 0, 255), 1) - if cv2.contourArea(cnt) > 1000: - x, y, w, h = cv2.boundingRect(cnt) - if draw_rectangles: - if (w * h) > area and max(w/h , h/w) < 5 and (w * h) < (window_height * window_width / 4): - cv2.rectangle(myObject, (x, y), (x + w, y + h), (0, 255, 0), 2) - - frame = cv2.hconcat([myObject, color_square]) - cv2.imshow('colorspace calibration', frame) - - if cv2.waitKey(1) & 0xff ==ord('q'): - df['hue_light'][0] = hue_light - df['sat_light'][0] = sat_light - df['val_light'][0] = val_light - df['hue_dark'][0] = hue_dark - df['sat_dark'][0] = sat_dark - df['val_dark'][0] = val_dark - df['area'][0] = area - df.to_csv('values.csv', index=False) - break -cv2.destroyAllWindows() - -# new: -import matplotlib.pyplot as plt -import matplotlib.colors -import numpy as np -import pandas as pd -import cv2 -import os -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") -df = pd.read_csv('values.csv') -default_hue_light = df['hue_light'][0] -default_sat_light = df['sat_light'][0] -default_val_light = df['val_light'][0] -default_hue_dark = df['hue_dark'][0] -default_sat_dark = df['sat_dark'][0] -default_val_dark = df['val_dark'][0] -default_area = df['area'][0] -height, width, _ = image.shape -window_height, window_width = height, width -hue_light = 10 -sat_light = 20 -val_light = 10 -hue_dark = 250 -sat_dark = 10 -val_dark = 250 -area = 100 -draw_countours = 1 -draw_rectangles = 1 - -def onTrack1(val): - global hue_light - hue_light=val -def onTrack2(val): - global hue_dark - hue_dark=val -def onTrack3(val): - global sat_light - sat_light=val -def onTrack4(val): - global sat_dark - sat_dark=val -def onTrack5(val): - global val_light - val_light=val -def onTrack6(val): - global val_dark - val_dark=val -def onTrack7(val): - global area - area=val -def onTrack8(val): - global draw_countours - draw_countours=val -def onTrack9(val): - global draw_rectangles - draw_rectangles=val - - -cv2.namedWindow('colorspace calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('colorspace calibration', window_width, window_height) -cv2.moveWindow('colorspace calibration',0,0) -cv2.createTrackbar('Hue Low','colorspace calibration',default_hue_light,179,onTrack1) -cv2.createTrackbar('Hue High','colorspace calibration',default_hue_dark,179,onTrack2) -cv2.createTrackbar('Sat Low','colorspace calibration',default_sat_light,255,onTrack3) -cv2.createTrackbar('Sat High','colorspace calibration',default_sat_dark,255,onTrack4) -cv2.createTrackbar('Val Low','colorspace calibration',default_val_light,255,onTrack5) -cv2.createTrackbar('Val High','colorspace calibration',default_val_dark,255,onTrack6) -cv2.createTrackbar('Area','colorspace calibration',default_area,window_height*window_width,onTrack7) -cv2.createTrackbar('Contours','colorspace calibration',1,1,onTrack8) -cv2.createTrackbar('Rectangles','colorspace calibration',1,1,onTrack9) - -while True: - frameHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV) - - lowerBound=np.array([hue_light,sat_light,val_light]) - upperBound=np.array([hue_dark,sat_dark,val_dark]) - - lo_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_light,sat_light,val_light], dtype=np.uint8) / 255.0 - do_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_dark,sat_dark,val_dark], dtype=np.uint8) / 255.0 - - lo_square_rgb = matplotlib.colors.hsv_to_rgb(lo_square) - do_square_rgb = matplotlib.colors.hsv_to_rgb(do_square) - - lo_square_bgr = cv2.cvtColor((lo_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - do_square_bgr = cv2.cvtColor((do_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - color_square = cv2.vconcat([lo_square_bgr, do_square_bgr]) + if len(files) == 0: + raise ValueError(f"No images files found in {image_path}") - myMask=cv2.inRange(frameHSV,lowerBound,upperBound) - myObject=cv2.bitwise_and(image,image,mask=myMask) - contours, _ = cv2.findContours(myMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - for cnt in contours: - if draw_countours: - cv2.drawContours(myObject, [cnt], -1, (0, 0, 255), 1) - if cv2.contourArea(cnt) > 1000: - x, y, w, h = cv2.boundingRect(cnt) - if draw_rectangles: - if (w * h) > area and max(w/h , h/w) < 5 and (w * h) < (window_height * window_width / 4): - cv2.rectangle(myObject, (x, y), (x + w, y + h), (0, 255, 0), 2) + image = cv2.imread(files[index % len(files)]) - frame = cv2.hconcat([myObject, color_square]) - cv2.imshow('colorspace calibration', frame) + height, width, _ = image.shape + window_height, window_width = height, width + + draw_counters = True + draw_rectangles = True + + def onTrack1(val): + global hue_light + hue_light=val + def onTrack2(val): + global hue_dark + hue_dark=val + def onTrack3(val): + global sat_light + sat_light=val + def onTrack4(val): + global sat_dark + sat_dark=val + def onTrack5(val): + global val_light + val_light=val + def onTrack6(val): + global val_dark + val_dark=val + def onTrack7(val): + global area + area=val + + cv2.namedWindow('colorspace calibration', cv2.WINDOW_NORMAL) + screen_width = cv2.getWindowImageRect('colorspace calibration')[2] + screen_height = cv2.getWindowImageRect('colorspace calibration')[3] + cv2.resizeWindow('colorspace calibration', screen_width, screen_height) + + cv2.createTrackbar('Hue Low','colorspace calibration',hue_light,179,onTrack1) + cv2.createTrackbar('Hue High','colorspace calibration',hue_dark,179,onTrack2) + cv2.createTrackbar('Sat Low','colorspace calibration',sat_light,255,onTrack3) + cv2.createTrackbar('Sat High','colorspace calibration',sat_dark,255,onTrack4) + cv2.createTrackbar('Val Low','colorspace calibration',val_light,255,onTrack5) + cv2.createTrackbar('Val High','colorspace calibration',val_dark,255,onTrack6) + cv2.createTrackbar('Area','colorspace calibration',default_area,window_height*window_width,onTrack7) - if cv2.waitKey(1) & 0xff ==ord('q'): - df['hue_light'][0] = hue_light - df['sat_light'][0] = sat_light - df['val_light'][0] = val_light - df['hue_dark'][0] = hue_dark - df['sat_dark'][0] = sat_dark - df['val_dark'][0] = val_dark - df['area'][0] = area - df.to_csv('values.csv', index=False) - break -cv2.destroyAllWindows() \ No newline at end of file + key = 0 + + while key != ord('q'): + frameHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV) + + lowerBound=np.array([hue_light,sat_light,val_light]) + upperBound=np.array([hue_dark,sat_dark,val_dark]) + + lo_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_light,sat_light,val_light], dtype=np.uint8) / 255.0 + do_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_dark,sat_dark,val_dark], dtype=np.uint8) / 255.0 + + lo_square_rgb = matplotlib.colors.hsv_to_rgb(lo_square) + do_square_rgb = matplotlib.colors.hsv_to_rgb(do_square) + + lo_square_bgr = cv2.cvtColor((lo_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) + do_square_bgr = cv2.cvtColor((do_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) + color_square = cv2.vconcat([lo_square_bgr, do_square_bgr]) + + myMask=cv2.inRange(frameHSV,lowerBound,upperBound) + myObject=cv2.bitwise_and(image,image,mask=myMask) + + contours, _ = cv2.findContours(myMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + for cnt in contours: + if draw_counters: + cv2.drawContours(myObject, [cnt], -1, (0, 0, 255), 1) + if cv2.contourArea(cnt) > 100: + x, y, w, h = cv2.boundingRect(cnt) + if draw_rectangles: + if (w * h) > area and max(w/h , h/w) < 5 and (w * h) < (window_height * window_width / 4): + cv2.rectangle(myObject, (x, y), (x + w, y + h), (0, 255, 0), 2) + + frame = cv2.hconcat([myObject, color_square]) + + cv2.imshow('colorspace calibration', frame) + + key = cv2.waitKey(1) + if key == ord('q'): + cv2.destroyAllWindows() + break + elif key == ord('c'): + draw_counters = not draw_counters + elif key == ord('r'): + draw_rectangles = not draw_rectangles + elif key == ord('a'): + index -= 1 + image = cv2.imread(files[index % len(files)]) + elif key == ord('d'): + index += 1 + image = cv2.imread(files[index % len(files)]) + +if __name__ == "__main__": + print("Usage: python3 colorspace_calibration.py --images_path=/home/user/Desktop/Applied-AI/ROS/assets/maize") + parser = argparse.ArgumentParser(description='Calibrate color filter boundaries') + parser.add_argument('--images_path', type=str, required=False, help='Path to the folder of calibration images') + args = parser.parse_args() + # color_calibrator(args.images_path) + color_calibrator() \ No newline at end of file diff --git a/calibration_tools/exporter_test.py b/calibration_tools/exporter_test.py deleted file mode 100644 index fd28927..0000000 --- a/calibration_tools/exporter_test.py +++ /dev/null @@ -1,16 +0,0 @@ -from ultralytics import YOLO -from ultralytics.utils.benchmarks import benchmark - -model = YOLO("/home/usr/Downloads/Maize.pt") - -model.export(format='engine', imgsz=640, half=False, dynamic=False, simplify=False, workspace=8.0, int8=False, batch=1) - -model.export(format='onnx', imgsz=640, half=False, dynamic=False, simplify=False, opset=12, batch=1) - -# onnxslim - -# GPU benchmarking... -benchmark(model="yolov8n.pt", data="coco8.yaml", imgsz=640, half=False, device=0) # verbose, int8 - -## onnx: imgsz, half, dynamic, simplify, opset, batch -# tensorrt: imgsz, half, dynamic, simplify, workspace, int8, batch \ No newline at end of file diff --git a/calibration_tools/nms_calibration.py b/calibration_tools/nms_calibration.py deleted file mode 100644 index c3d9aae..0000000 --- a/calibration_tools/nms_calibration.py +++ /dev/null @@ -1,83 +0,0 @@ -import cv2 -import numpy as np - -# Generate random bounding boxes for demonstration purposes -def generate_random_boxes(image_shape, num_boxes=10): - boxes = [] - for _ in range(num_boxes): - x1 = np.random.randint(0, image_shape[1] - 50) - y1 = np.random.randint(0, image_shape[0] - 50) - x2 = np.random.randint(x1 + 30, min(image_shape[1], x1 + 100)) - y2 = np.random.randint(y1 + 30, min(image_shape[0], y1 + 100)) - boxes.append([x1, y1, x2, y2]) - return np.array(boxes) - -# Apply Non-Maximum Suppression -def nms(boxes, threshold): - if len(boxes) == 0: - return [] - - boxes = boxes.astype(np.float32) - pick = [] - - x1 = boxes[:, 0] - y1 = boxes[:, 1] - x2 = boxes[:, 2] - y2 = boxes[:, 3] - - area = (x2 - x1 + 1) * (y2 - y1 + 1) - idxs = np.argsort(y2) - - while len(idxs) > 0: - last = len(idxs) - 1 - i = idxs[last] - pick.append(i) - - xx1 = np.maximum(x1[i], x1[idxs[:last]]) - yy1 = np.maximum(y1[i], y1[idxs[:last]]) - xx2 = np.minimum(x2[i], x2[idxs[:last]]) - yy2 = np.minimum(y2[i], y2[idxs[:last]]) - - w = np.maximum(0, xx2 - xx1 + 1) - h = np.maximum(0, yy2 - yy1 + 1) - - overlap = (w * h) / area[idxs[:last]] - - idxs = np.delete(idxs, np.concatenate(([last], np.where(overlap > threshold)[0]))) - - return boxes[pick].astype(int) - -# Draw bounding boxes on an image -def draw_boxes(image, boxes, color=(0, 255, 0), thickness=2): - for (x1, y1, x2, y2) in boxes: - cv2.rectangle(image, (x1, y1), (x2, y2), color, thickness) - -# Callback function for trackbar -def update_threshold(x): - global img_copy, original_boxes - img_copy = img.copy() - threshold = cv2.getTrackbarPos('NMS Threshold', 'Calibration Tool') / 100 - selected_boxes = nms(original_boxes, threshold) - draw_boxes(img_copy, original_boxes, color=(0, 0, 255)) # Draw original boxes in red - draw_boxes(img_copy, selected_boxes) # Draw NMS-selected boxes in green - cv2.imshow('Calibration Tool', img_copy) - -# Load your image -img = cv2.imread('image.jpg') -img_copy = img.copy() - -# Generate random bounding boxes -original_boxes = generate_random_boxes(img.shape) - -# Create a window -cv2.namedWindow('Calibration Tool') - -# Create a trackbar for threshold adjustment -cv2.createTrackbar('NMS Threshold', 'Calibration Tool', 50, 100, update_threshold) - -# Initial display -update_threshold(50) - -# Wait until user exits the window -cv2.waitKey(0) -cv2.destroyAllWindows() diff --git a/calibration_tools/roi_calibration.py b/calibration_tools/roi_calibration.py index 90f8b64..cf9006a 100644 --- a/calibration_tools/roi_calibration.py +++ b/calibration_tools/roi_calibration.py @@ -1,244 +1,234 @@ -import pyzed.sl as sl +# import pyzed.sl as sl import matplotlib.pyplot as plt import numpy as np import cv2 import os -import pandas as pd - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -df = pd.read_csv('values.csv') - -default_brightness = df['brightness'][0] -default_contrast = df['contrast'][0] -default_hue = df['hue'][0] -default_saturation = df['saturation'][0] -default_sharpness = df['sharpness'][0] -default_gamma = df['gamma'][0] -default_gain = df['gain'][0] -default_whitebalance = df['whitebalance'][0] -default_x = df['x'][0] -default_y = df['y'][0] -default_w = df['w'][0] -default_h = df['h'][0] - -def onTrack1(val): - global default_x - default_x=val - print('roi x',default_x) -def onTrack2(val): - global default_w - default_w=val - print('roi w',default_w) -def onTrack3(val): - global default_y - default_y=val - print('roi y',default_y) -def onTrack4(val): - global default_h - default_h=val - print('roi h',default_h) -def onTrack4(val): - global default_h - default_h=val - print('roi h',default_h) - -cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('roi calibration', window_width, window_height) -cv2.moveWindow('roi calibration',0,0) - -cv2.createTrackbar('x pos','roi calibration',1,width-1,onTrack1) -cv2.createTrackbar('width','roi calibration',width,width,onTrack2) -cv2.createTrackbar('y pos','roi calibration',1,height-1,onTrack3) -cv2.createTrackbar('height','roi calibration',height,height,onTrack4) - -init = sl.InitParameters() -cam = sl.Camera() -init.camera_resolution = sl.RESOLUTION.HD1080 -init.camera_fps = 30 - -if not cam.is_opened(): - print("Opening ZED Camera ") -status = cam.open(init) -if status != sl.ERROR_CODE.SUCCESS: - print(repr(status)) - exit() - -runtime = sl.RuntimeParameters() -mat = sl.Mat() +import time +import argparse + +roi_x = 0 +roi_y = 0 +roi_w = 100 +roi_h = 100 +previous_velocity_left = np.array([0.0, 0.0, 0.0]) +previous_velocity_right = np.array([0.0, 0.0, 0.0]) +previous_time_left = time.time() +previous_time_right = time.time() +velocity = 1 +shift_constant = 1 + +def initialize_image_source(source_type="static_image", image_path='C:/Users/ishaa/Coding Projects/Applied-AI/ROS/assets/maize'): + if source_type == "static_image": + if not os.path.exists(image_path): + raise ValueError(f"Images folder not found at {image_path}") + if len(os.listdir(image_path)) == 0: + raise ValueError(f"Images folder is empty") + + files = [] + os.chdir(image_path) + for filename in os.listdir(image_path): + if filename.endswith(".jpg") or filename.endswith(".JPG") or filename.endswith(".png"): + files.append(image_path + '/' + filename) + + if len(files) == 0: + raise ValueError(f"No images files found in {image_path}") + + return files + + elif source_type == "zed_single": + init = sl.InitParameters() + cam = sl.Camera() + init.camera_resolution = sl.RESOLUTION.HD1080 + init.camera_fps = 30 + + if not cam.is_opened(): + print("Opening ZED Camera ") + status = cam.open(init) + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + raise ValueError("Error opening ZED Camera") + return cam + + elif source_type == "zed_double": + init = sl.InitParameters() + cam1 = sl.Camera() + cam2 = sl.Camera() + init.camera_resolution = sl.RESOLUTION.HD1080 + init.camera_fps = 30 + + init.camera_linux_id = 0 + if not cam1.is_opened(): + print("Opening ZED Camera ") + status = cam1.open(init) + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + raise ValueError("Error opening first ZED Camera") + + init.camera_linux_id = 1 + if not cam2.is_opened(): + print("Opening ZED Camera ") + status = cam2.open(init) + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + raise ValueError("Error opening second ZED Camera") + + return cam1, cam2 + + else: + raise ValueError("Invalid source type") -while True: +def retrieve_zed_image(cam, orientation="left"): + global previous_velocity_left, previous_velocity_right + global previous_time_left, previous_time_right + + current_time = time.time() + runtime = sl.RuntimeParameters() + sensor = sl.SensorsData() + mat = sl.Mat() + err = cam.grab(runtime) if err == sl.ERROR_CODE.SUCCESS: - cam.retrieve_image(mat, sl.VIEW.LEFT_UNRECTIFIED) + if orientation == "left": + cam.retrieve_image(mat, sl.VIEW.LEFT_UNRECTIFIED) + else: + cam.retrieve_image(mat, sl.VIEW.RIGHT_UNRECTIFIED) + image = mat.get_data() image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) + cam.get_sensors_data(sensor, sl.TIME_REFERENCE.IMAGE) + accel_data = sensor.get_imu_data().get_linear_acceleration() + acceleration = np.array([accel_data[0], accel_data[1], accel_data[2]]) + + if orientation == "left": + current_time = time.time() + delta_time = current_time - previous_time_left + velocity = previous_velocity_left + (acceleration * delta_time) + previous_velocity_left = velocity + previous_time_left = current_time + else: + current_time = time.time() + delta_time = current_time - previous_time_right + velocity = previous_velocity_right + (acceleration * delta_time) + previous_velocity_right = velocity + previous_time_right = current_time + + else: + raise ValueError("Error grabbing image from ZED Camera") - frame = image[default_y:default_h+1, default_x:default_w+1] + return image, velocity[0] + +def roi_calibrator(source_type="static_image", images_path='C:/Users/ishaa/Coding Projects/Applied-AI/ROS/assets/maize'): + global roi_x, roi_y, roi_w, roi_h, shift_constant, velocity + image_source = initialize_image_source(source_type, images_path) + if source_type == "zed_double": + cam1, cam2 = image_source + height, width = 1080, 1920 + elif source_type == "zed_single": + cam = image_source + height, width = 1080, 1920 + else: + height, width, _ = cv2.imread(image_source[0]).shape + + key = 0 + image = None + index = 0 + window_height, window_width = height, width + roi_w, roi_h = width, height + orientation = "left" + + def onTrack1(val): + global roi_x + roi_x=val + def onTrack2(val): + global roi_w + roi_w=val + def onTrack3(val): + global roi_y + roi_y=val + def onTrack4(val): + global roi_h + roi_h=val + def onTrack5(val): + global shift_constant + shift_constant=val + def onTrack6(val): + global velocity + velocity=val - blank_image = np.zeros((window_height, window_width, 3), np.uint8) - x_offset = max((window_width - frame.shape[1]) // 2, 0) - y_offset = max((window_height - frame.shape[0]) // 2, 0) + cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) + cv2.resizeWindow('roi calibration', window_width, window_height) + cv2.moveWindow('roi calibration',0,0) + + cv2.createTrackbar('x pos','roi calibration',roi_x,(width-1),onTrack1) + cv2.createTrackbar('width','roi calibration',roi_w,(width),onTrack2) + cv2.createTrackbar('y pos','roi calibration',roi_y,(height-1),onTrack3) + cv2.createTrackbar('height','roi calibration',roi_h,(height),onTrack4) + cv2.createTrackbar('shift','roi calibration',shift_constant,1000,onTrack5) - blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame + if source_type == "static_image": + cv2.createTrackbar('velocity','roi calibration',velocity,100,onTrack6) - cv2.imshow('roi calibration', blank_image) - if cv2.waitKey(1) & 0xff ==ord('q'): - break - -cam.close() -print("ZED Camera closed") -cv2.destroyAllWindows() - -# old -import matplotlib.pyplot as plt -import numpy as np -import cv2 -import os - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -height, width, _ = image.shape -window_height, window_width = height, width - -roi_x = 1 -roi_w = width -roi_y = 1 -roi_h = height - -def onTrack1(val): - global roi_x - roi_x=val - print('roi x',roi_x) -def onTrack2(val): - global roi_w - roi_w=val - print('roi w',roi_w) -def onTrack3(val): - global roi_y - roi_y=val - print('roi y',roi_y) -def onTrack4(val): - global roi_h - roi_h=val - print('roi h',roi_h) - -cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('roi calibration', window_width, window_height) -cv2.moveWindow('roi calibration',0,0) - -cv2.createTrackbar('x pos','roi calibration',1,width-1,onTrack1) -cv2.createTrackbar('width','roi calibration',width,width,onTrack2) -cv2.createTrackbar('y pos','roi calibration',1,height-1,onTrack3) -cv2.createTrackbar('height','roi calibration',height,height,onTrack4) - -while True: - frame = image[roi_y:roi_h+1, roi_x:roi_w+1] - - blank_image = np.zeros((window_height, window_width, 3), np.uint8) - x_offset = max((window_width - frame.shape[1]) // 2, 0) - y_offset = max((window_height - frame.shape[0]) // 2, 0) - - blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame - - cv2.imshow('roi calibration', blank_image) - if cv2.waitKey(1) & 0xff ==ord('q'): - break - -cv2.destroyAllWindows() - -# newest -import pyzed.sl as sl -import matplotlib.pyplot as plt -import numpy as np -import cv2 -import os -import pandas as pd - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") -image = cv2.imread("test.jpg") - -df = pd.read_csv('values.csv') - -default_brightness = df['brightness'][0] -default_contrast = df['contrast'][0] -default_hue = df['hue'][0] -default_saturation = df['saturation'][0] -default_sharpness = df['sharpness'][0] -default_gamma = df['gamma'][0] -default_gain = df['gain'][0] -default_whitebalance = df['whitebalance'][0] -default_x = df['x'][0] -default_y = df['y'][0] -default_w = df['w'][0] -default_h = df['h'][0] - -def onTrack1(val): - global default_x - default_x=val - print('roi x',default_x) -def onTrack2(val): - global default_w - default_w=val - print('roi w',default_w) -def onTrack3(val): - global default_y - default_y=val - print('roi y',default_y) -def onTrack4(val): - global default_h - default_h=val - print('roi h',default_h) -def onTrack4(val): - global default_h - default_h=val - print('roi h',default_h) - -cv2.namedWindow('roi calibration', cv2.WINDOW_NORMAL) -cv2.resizeWindow('roi calibration', window_width, window_height) -cv2.moveWindow('roi calibration',0,0) - -cv2.createTrackbar('x pos','roi calibration',1,width-1,onTrack1) -cv2.createTrackbar('width','roi calibration',width,width,onTrack2) -cv2.createTrackbar('y pos','roi calibration',1,height-1,onTrack3) -cv2.createTrackbar('height','roi calibration',height,height,onTrack4) - -init = sl.InitParameters() -cam = sl.Camera() -init.camera_resolution = sl.RESOLUTION.HD1080 -init.camera_fps = 30 - -if not cam.is_opened(): - print("Opening ZED Camera ") -status = cam.open(init) -if status != sl.ERROR_CODE.SUCCESS: - print(repr(status)) - exit() - -runtime = sl.RuntimeParameters() -mat = sl.Mat() - -while True: - err = cam.grab(runtime) - if err == sl.ERROR_CODE.SUCCESS: - cam.retrieve_image(mat, sl.VIEW.LEFT_UNRECTIFIED) - image = mat.get_data() - image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) - - frame = image[default_y:default_h+1, default_x:default_w+1] - - blank_image = np.zeros((window_height, window_width, 3), np.uint8) - x_offset = max((window_width - frame.shape[1]) // 2, 0) - y_offset = max((window_height - frame.shape[0]) // 2, 0) - - blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame - - cv2.imshow('roi calibration', blank_image) - if cv2.waitKey(1) & 0xff ==ord('q'): - break - -cam.close() -print("ZED Camera closed") -cv2.destroyAllWindows() \ No newline at end of file + while key != ord('q'): + if source_type == "static_image": + image = cv2.imread(image_source[index % len(image_source)]) + shifted_roi_x = roi_x + abs(velocity) * shift_constant + + elif source_type == "zed_single": + image, velocity = retrieve_zed_image(cam, orientation) + if orientation == "left": + shifted_roi_x = roi_x + abs(velocity) * shift_constant + else: + image = cv2.rotate(image, cv2.ROTATE_180) + image = cv2.flip(image, 1) + shifted_roi_x = roi_x - abs(velocity) * shift_constant + + else: + if orientation == "left": + image, velocity = retrieve_zed_image(cam1, orientation) + shifted_roi_x = roi_x + abs(velocity) * shift_constant + else: + image, velocity = retrieve_zed_image(cam2, orientation) + shifted_roi_x = roi_x - abs(velocity) * shift_constant + + # Ensure ROI stays within bounds + roi_x_max = min(shifted_roi_x + roi_w, width) + roi_y_max = min(roi_y + roi_h, height) + + # Crop the image to the ROI + frame = image[roi_y:roi_y_max, shifted_roi_x:roi_x_max] + + # Create a blank image to center the ROI if it's smaller than the window + blank_image = np.zeros((window_height, window_width, 3), np.uint8) + x_offset = max((window_width - frame.shape[1]) // 2, 0) + y_offset = max((window_height - frame.shape[0]) // 2, 0) + + # Paste the cropped frame onto the blank image + blank_image[y_offset:y_offset+frame.shape[0], x_offset:x_offset+frame.shape[1]] = frame + + cv2.imshow('roi calibration', blank_image) + + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + if source_type == "zed_single": + cam.close() + elif source_type == "zed_double": + cam1.close() + cam2.close() + cv2.destroyAllWindows() + elif key == ord('l'): + orientation = "left" + elif key == ord('r'): + orientaion = "right" + elif key == ord('a'): + index -= 1 + elif key == ord('d'): + index += 1 + +if __name__ == "__main__": + print("Usage: python3 roi_calibration.py --source_type=static_image --images_path=/home/user/Desktop/Applied-AI/ROS/assets/maize") + parser = argparse.ArgumentParser(description='Calibrate color filter boundaries') + parser.add_argument('--source_type', type=str, required=False, help='Type of image source (static_image, zed_single, zed_double)') + parser.add_argument('--images_path', type=str, required=False, help='Path to the folder of calibration images') + args = parser.parse_args() + # roi_calibrator(args.source_type, args.images_path) + roi_calibrator() \ No newline at end of file diff --git a/calibration_tools/test.jpg b/calibration_tools/test.jpg deleted file mode 100644 index 892ee7e..0000000 Binary files a/calibration_tools/test.jpg and /dev/null differ diff --git a/calibration_tools/test.py b/calibration_tools/test.py deleted file mode 100644 index 2dc8d59..0000000 --- a/calibration_tools/test.py +++ /dev/null @@ -1,111 +0,0 @@ -import matplotlib.pyplot as plt -import matplotlib.colors -import numpy as np -import pandas as pd -import cv2 -import os - -os.chdir("C:/Users/ishaa/Coding Projects/Applied-AI/ROS/calibration_tools") -image = cv2.imread("test.jpg") - -df = pd.read_csv('values.csv') - -default_hue_light = df['hue_light'][0] -default_sat_light = df['sat_light'][0] -default_val_light = df['val_light'][0] -default_hue_dark = df['hue_dark'][0] -default_sat_dark = df['sat_dark'][0] -default_val_dark = df['val_dark'][0] -default_area = df['area'][0] - -height, width, _ = image.shape -window_height, window_width = height, width - -hue_light = 10 -sat_light = 20 -val_light = 10 -hue_dark = 250 -sat_dark = 10 -val_dark = 250 -area = 100 -def onTrack1(val): - global hue_light - hue_light=val - print('Hue Low',hue_light) -def onTrack2(val): - global hue_dark - hue_dark=val - print('Hue High',hue_dark) -def onTrack3(val): - global sat_light - sat_light=val - print('Sat Low',sat_light) -def onTrack4(val): - global sat_dark - sat_dark=val - print('Sat High',sat_dark) -def onTrack5(val): - global val_light - val_light=val - print('Val Low',val_light) -def onTrack6(val): - global val_dark - val_dark=val - print('Val High',val_dark) -def onTrack7(val): - global area - area=val - print('Area',area) -cv2.namedWindow('colorspace calibration', cv2.WINDOW_NORMAL) - -cv2.resizeWindow('colorspace calibration', window_width, window_height) -cv2.moveWindow('colorspace calibration',0,0) - -cv2.createTrackbar('Hue Low','colorspace calibration',default_hue_light,179,onTrack1) -cv2.createTrackbar('Hue High','colorspace calibration',default_hue_dark,179,onTrack2) -cv2.createTrackbar('Sat Low','colorspace calibration',default_sat_light,255,onTrack3) -cv2.createTrackbar('Sat High','colorspace calibration',default_sat_dark,255,onTrack4) -cv2.createTrackbar('Val Low','colorspace calibration',default_val_light,255,onTrack5) -cv2.createTrackbar('Val High','colorspace calibration',default_val_dark,255,onTrack6) -cv2.createTrackbar('Area','colorspace calibration',default_area,window_height*window_width,onTrack7) - -while True: - frameHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV) - - lowerBound=np.array([hue_light,sat_light,val_light]) - upperBound=np.array([hue_dark,sat_dark,val_dark]) - - lo_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_light,sat_light,val_light], dtype=np.uint8) / 255.0 - do_square = np.full((int(window_height/2), int(window_width//5), 3), [hue_dark,sat_dark,val_dark], dtype=np.uint8) / 255.0 - lo_square_rgb = matplotlib.colors.hsv_to_rgb(lo_square) - do_square_rgb = matplotlib.colors.hsv_to_rgb(do_square) - lo_square_bgr = cv2.cvtColor((lo_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - do_square_bgr = cv2.cvtColor((do_square_rgb* 255).astype('uint8'), cv2.COLOR_RGB2BGR) - color_square = cv2.vconcat([lo_square_bgr, do_square_bgr]) - - myMask=cv2.inRange(frameHSV,lowerBound,upperBound) - myObject=cv2.bitwise_and(image,image,mask=myMask) - contours, _ = cv2.findContours(myMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - for cnt in contours: - cv2.drawContours(myObject, [cnt], -1, (0, 0, 255), 1) - if cv2.contourArea(cnt) > 100: - x, y, w, h = cv2.boundingRect(cnt) - if (w * h) > area and max(w/h , h/w) < 5 and (w * h) < (window_height * window_width / 4): - print(w*h) - cv2.rectangle(myObject, (x, y), (x + w, y + h), (0, 255, 0), 2) - - frame = cv2.hconcat([myObject, color_square]) - cv2.imshow('colorspace calibration', frame) - - if cv2.waitKey(1) & 0xff ==ord('q'): - df['hue_light'][0] = hue_light - df['sat_light'][0] = sat_light - df['val_light'][0] = val_light - df['hue_dark'][0] = hue_dark - df['sat_dark'][0] = sat_dark - df['val_dark'][0] = val_dark - df['area'][0] = area - df.to_csv('values.csv', index=False) - break - -cv2.destroyAllWindows() \ No newline at end of file diff --git a/calibration_tools/values.csv b/calibration_tools/values.csv deleted file mode 100644 index 718e23c..0000000 --- a/calibration_tools/values.csv +++ /dev/null @@ -1,2 +0,0 @@ -hue_light,sat_light,val_light,hue_dark,sat_dark,val_dark,area -13,44,44,45,83,255,17806 diff --git a/conversion_tools/ONNX_Verify.py b/conversion_tools/ONNX_Verify.py index 3ca7703..b366a17 100644 --- a/conversion_tools/ONNX_Verify.py +++ b/conversion_tools/ONNX_Verify.py @@ -21,6 +21,12 @@ def verify_onnx(model_path, compared_outputs, model_dimensions, fp_16): # any other chanes for fp_16 to work? def predict_onnx(model_path, fp_16, input_shape): + + # random_input = np.random.randn(*input_shape).astype(np.float32) + + # # Run inference with ONNX + # input_name = onnx_session.get_inputs()[0].name + # onnx_output = onnx_session.run(None, {input_name: random_input}) onnx_session = ort.InferenceSession(model_path,providers=["CUDAExecutionProvider"]) if fp_16: diff --git a/conversion_tools/TRT_Benchmark.py b/conversion_tools/TRT_Benchmark.py index b1fd44c..8a08abe 100644 --- a/conversion_tools/TRT_Benchmark.py +++ b/conversion_tools/TRT_Benchmark.py @@ -6,6 +6,9 @@ import time import torch +## need to adapt after finalizing preprocessing/postprocessing steps +# should also do unit testing, but toggle functionality w/ param + # allocates input/ouput buffers for the TensorRT engine inference def allocate_buffers(engine): inputs = [] @@ -542,4 +545,6 @@ def run_benchmark(trt_model_path, test_images, ground_truth_path): test_images = ["test1.jpg", "test2.jpg"] # Replace with your test images ground_truth_path = "ground_truth.txt" # Replace with your ground truth file path - run_benchmark(trt_model_path, test_images, ground_truth_path) \ No newline at end of file + run_benchmark(trt_model_path, test_images, ground_truth_path) + +# Create performance report based on relative bounding box centroid for sample images (accuracy %, error offset %, etc.) \ No newline at end of file diff --git a/cpp_wip/component_ex.cpp b/cpp_wip/component_ex.cpp new file mode 100644 index 0000000..c5569a0 --- /dev/null +++ b/cpp_wip/component_ex.cpp @@ -0,0 +1,14 @@ +#include "rclcpp/rclcpp.hpp" + +class MyNode : public rclcpp::Node { +public: + MyNode(const rclcpp::NodeOptions & options) + : Node("my_node", options) { + RCLCPP_INFO(this->get_logger(), "MyNode started."); + } +}; + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(MyNode) + +// This example defines a simple component node that can be dynamically loaded into a container. \ No newline at end of file diff --git a/cpp_wip/composable_container.cpp b/cpp_wip/composable_container.cpp new file mode 100644 index 0000000..b4184a7 --- /dev/null +++ b/cpp_wip/composable_container.cpp @@ -0,0 +1,11 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/component_manager.hpp" + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + auto options = rclcpp::NodeOptions(); + auto component_manager = std::make_shared(options); + rclcpp::spin(component_manager); + rclcpp::shutdown(); + return 0; +} diff --git a/cpp_wip/cuda_pass.cpp b/cpp_wip/cuda_pass.cpp new file mode 100644 index 0000000..5b5cc01 --- /dev/null +++ b/cpp_wip/cuda_pass.cpp @@ -0,0 +1,21 @@ +// Composable Node 1: Image Preprocessing + +// This node would take input data, allocate GPU memory for the image, and perform preprocessing in CUDA. + +class ImagePreprocessingNode : public rclcpp::Node { +public: + ImagePreprocessingNode() : Node("image_preprocessing_node") { + // Allocate GPU memory for image + cudaMalloc(&d_image, width * height * sizeof(float)); + // Perform preprocessing (e.g., normalization) in CUDA + preprocess_image<<>>(d_image, width, height); + } + + // Function to get CUDA pointer to the preprocessed image + float* get_preprocessed_image() { + return d_image; // Return pointer to GPU memory + } + +private: + float* d_image; // CUDA pointer for image +}; diff --git a/cpp_wip/cuda_receive.cpp b/cpp_wip/cuda_receive.cpp new file mode 100644 index 0000000..2450f6f --- /dev/null +++ b/cpp_wip/cuda_receive.cpp @@ -0,0 +1,18 @@ +// Composable Node 2: Inference Using TensorRT + +// This node would receive the CUDA pointer from Node 1 and use it directly as input to the inference engine, avoiding any memory copy or serialization overhead. + +class InferenceNode : public rclcpp::Node { +public: + InferenceNode() : Node("inference_node") { + // Set up TensorRT engine and allocate necessary memory + } + + void run_inference(float* d_preprocessed_image) { + // Run inference directly on the GPU memory + trt_inference(d_preprocessed_image); + } + +private: + // TensorRT inference engine and buffers +}; diff --git a/cpp_wip/lcuda_pass_launch.py b/cpp_wip/lcuda_pass_launch.py new file mode 100644 index 0000000..2f65e62 --- /dev/null +++ b/cpp_wip/lcuda_pass_launch.py @@ -0,0 +1,29 @@ +# Both nodes can be loaded into a composable container, and the preprocessed CUDA memory pointer can be passed directly from ImagePreprocessingNode to InferenceNode. + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='gpu_composable_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', # Multi-threaded container for composable nodes + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='my_package::ImagePreprocessingNode', + name='image_preprocessing_node' + ), + ComposableNode( + package='my_package', + plugin='my_package::InferenceNode', + name='inference_node' + ), + ], + output='screen', + ), + ]) + +# In this setup, ImagePreprocessingNode preprocesses the image on the GPU, and InferenceNode receives the same CUDA memory buffer without any overhead due to inter-process communication or copying back and forth between GPU and CPU. The nodes remain within the same process and can share GPU memory pointers efficiently. \ No newline at end of file diff --git a/cpp_wip/lifecycle_compose.cpp b/cpp_wip/lifecycle_compose.cpp new file mode 100644 index 0000000..f661b46 --- /dev/null +++ b/cpp_wip/lifecycle_compose.cpp @@ -0,0 +1,58 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class MyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { +public: + MyLifecycleNode(const rclcpp::NodeOptions & options) + : rclcpp_lifecycle::LifecycleNode("my_lifecycle_node", options) {} + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Configuring..."); + // Initialization code + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Activating..."); + // Activation code + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Deactivating..."); + // Deactivation code + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Cleaning up..."); + // Cleanup code + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(this->get_logger(), "Shutting down..."); + // Shutdown code + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } +}; + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(MyLifecycleNode) + +// This code defines a lifecycle node that follows the component pattern, making it compatible with ROS2 composition. + +// Load the Lifecycle Node Dynamically: You can load the lifecycle node into a container just like any other composable node. Use the same launch file as before, but specify the lifecycle node's package and plugin. + +// Manage the Node’s Lifecycle: You can manage the node's state transitions using ROS2 lifecycle commands: + +// ros2 lifecycle set /my_lifecycle_node configure +// ros2 lifecycle set /my_lifecycle_node activate +// ros2 lifecycle set /my_lifecycle_node deactivate +// ros2 lifecycle set /my_lifecycle_node cleanup +// ros2 lifecycle set /my_lifecycle_node shutdown \ No newline at end of file diff --git a/cpp_wip/sample_cmake.txt b/cpp_wip/sample_cmake.txt new file mode 100644 index 0000000..f266648 --- /dev/null +++ b/cpp_wip/sample_cmake.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(my_package) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) + +add_library(my_cpp_node1 SHARED src/my_cpp_node1.cpp) +target_include_directories(my_cpp_node1 PRIVATE include) +ament_target_dependencies(my_cpp_node1 rclcpp rclcpp_components) + +add_library(my_cpp_node2 SHARED src/my_cpp_node2.cpp) +target_include_directories(my_cpp_node2 PRIVATE include) +ament_target_dependencies(my_cpp_node2 rclcpp rclcpp_components) + +rclcpp_components_register_nodes(my_cpp_node1 "my_package::MyCppNode1") +rclcpp_components_register_nodes(my_cpp_node2 "my_package::MyCppNode2") + +install(TARGETS + my_cpp_node1 + my_cpp_node2 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ DESTINATION include/) +install(PROGRAMS + scripts/my_python_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() + +# for cross compile python w/ cpp +add_library: Creates shared libraries for C++ nodes. +rclcpp_components_register_nodes: Registers C++ components for composition. +install: Installs binaries and scripts. \ No newline at end of file diff --git a/cpp_wip/sample_xml.txt b/cpp_wip/sample_xml.txt new file mode 100644 index 0000000..ebb308a --- /dev/null +++ b/cpp_wip/sample_xml.txt @@ -0,0 +1,20 @@ + + my_package + 0.0.1 + My ROS2 package with C++ and Python nodes + Your Name + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_components + python3 + + + ament_cmake + + + +: Specifies dependencies for both C++ and Python. +: Specifies runtime dependencies like Python. \ No newline at end of file diff --git a/python_wip/Local_Test.py b/python_wip/Local_Test.py deleted file mode 100644 index 6420e0a..0000000 --- a/python_wip/Local_Test.py +++ /dev/null @@ -1,58 +0,0 @@ -import cv2 -import os - -def draw_bounding_boxes(image_path, bboxes): - # Read the image using OpenCV - img = cv2.imread(image_path) - - if img is None: - print(f"Error loading image: {image_path}") - return - - # Get the dimensions of the image - height, width, _ = img.shape - print(height) - print(width) - - # Draw each bounding box on the image - for bbox in bboxes: - class_id, x_center, y_center, bbox_width, bbox_height = bbox - - # Convert normalized values to absolute pixel values - x_center_pixel = int(x_center * width) - y_center_pixel = int(y_center * height) - bbox_width_pixel = int(bbox_width * width) - bbox_height_pixel = int(bbox_height * height) - - # Calculate the top-left and bottom-right corners of the bounding box - top_left_x = int(x_center_pixel - bbox_width_pixel / 2) - top_left_y = int(y_center_pixel - bbox_height_pixel / 2) - bottom_right_x = int(x_center_pixel + bbox_width_pixel / 2) - bottom_right_y = int(y_center_pixel + bbox_height_pixel / 2) - - # Draw the bounding box (using green color and thickness of 2) - cv2.rectangle(img, (top_left_x, top_left_y), (bottom_right_x, bottom_right_y), (0, 255, 0), 2) - - # Show the image with bounding boxes (press any key to close) - cv2.imshow('Bounding Boxes', img) - cv2.waitKey(10000) - cv2.destroyAllWindows() - -def read_bounding_boxes(txt_file): - bboxes = [] - with open(txt_file, 'r') as file: - for line in file.readlines(): - values = line.strip().split() - class_id = int(values[0]) - x_center = float(values[1]) - y_center = float(values[2]) - bbox_width = float(values[3]) - bbox_height = float(values[4]) - bboxes.append((class_id, x_center, y_center, bbox_width, bbox_height)) - return bboxes - -os.chdir("C:/Users/Ishaan/Coding Projects/Applied-AI/ROS/Models/Maize Model/sample_maize_images") -print(os.getcwd()) -boxes = read_bounding_boxes("IMG_2884_18.txt") -print(boxes) -draw_bounding_boxes("IMG_2884_18.JPG", boxes) \ No newline at end of file diff --git a/python_wip/TF Benchmarking Example.py b/python_wip/TF Benchmarking Example.py deleted file mode 100644 index 10df1ff..0000000 --- a/python_wip/TF Benchmarking Example.py +++ /dev/null @@ -1,80 +0,0 @@ -# https://github.com/tensorflow/tensorrt/blob/master/tftrt/examples-py/PTQ_example.ipynb -model = tf.keras.models.load_model('resnet50_saved_model') - -batch_size = 8 -batched_input = np.zeros((batch_size, 224, 224, 3), dtype=np.float32) - -for i in range(batch_size): - img_path = './data/img%d.JPG' % (i % 4) - img = image.load_img(img_path, target_size=(224, 224)) - x = image.img_to_array(img) - x = np.expand_dims(x, axis=0) - x = preprocess_input(x) - batched_input[i, :] = x -batched_input = tf.constant(batched_input) -print('batched_input shape: ', batched_input.shape) - -# Benchmarking throughput -N_warmup_run = 50 -N_run = 1000 -elapsed_time = [] - -for i in range(N_warmup_run): - preds = model.predict(batched_input) - -for i in range(N_run): - start_time = time.time() - preds = model.predict(batched_input) - end_time = time.time() - elapsed_time = np.append(elapsed_time, end_time - start_time) - if i % 50 == 0: - print('Step {}: {:4.1f}ms'.format(i, (elapsed_time[-50:].mean()) * 1000)) - -print('Throughput: {:.0f} images/s'.format(N_run * batch_size / elapsed_time.sum())) - -def predict_tftrt(input_saved_model): - """Runs prediction on a single image and shows the result. - input_saved_model (string): Name of the input model stored in the current dir - """ - img_path = './data/img0.JPG' # Siberian_husky - img = image.load_img(img_path, target_size=(224, 224)) - x = image.img_to_array(img) - x = np.expand_dims(x, axis=0) - x = preprocess_input(x) - x = tf.constant(x) - - saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) - signature_keys = list(saved_model_loaded.signatures.keys()) - print(signature_keys) - - infer = saved_model_loaded.signatures['serving_default'] - print(infer.structured_outputs) - - labeling = infer(x) - preds = labeling['predictions'].numpy() - print('{} - Predicted: {}'.format(img_path, decode_predictions(preds, top=3)[0])) - plt.subplot(2,2,1) - plt.imshow(img); - plt.axis('off'); - plt.title(decode_predictions(preds, top=3)[0][0][1]) - -def benchmark_tftrt(input_saved_model): - saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) - infer = saved_model_loaded.signatures['serving_default'] - - N_warmup_run = 50 - N_run = 1000 - elapsed_time = [] - - for i in range(N_warmup_run): - labeling = infer(batched_input) - - for i in range(N_run): - start_time = time.time() - labeling = infer(batched_input) - end_time = time.time() - elapsed_time = np.append(elapsed_time, end_time - start_time) - if i % 50 == 0: - print('Step {}: {:4.1f}ms'.format(i, (elapsed_time[-50:].mean()) * 1000)) - - print('Throughput: {:.0f} images/s'.format(N_run * batch_size / elapsed_time.sum())) \ No newline at end of file diff --git a/python_wip/TF_TRT Inference.py b/python_wip/TF_TRT Inference.py index 5ec377a..0eba0a9 100644 --- a/python_wip/TF_TRT Inference.py +++ b/python_wip/TF_TRT Inference.py @@ -69,8 +69,8 @@ def convert(self, output_saved_model_dir, precision="FP32", max_workspace_size_b trt_precision = precision_dict[precision] conversion_params = tf_trt.DEFAULT_TRT_CONVERSION_PARAMS._replace(precision_mode=trt_precision, - max_workspace_size_bytes=max_workspace_size_bytes, - use_calibration= precision == "INT8") + max_workspace_size_bytes=max_workspace_size_bytes, + use_calibration= precision == "INT8") converter = tf_trt.TrtGraphConverterV2(input_saved_model_dir=self.input_saved_model_dir, conversion_params=conversion_params) @@ -90,4 +90,216 @@ def predict(self, input_data): return self.loaded_model.predict(input_data) def load_default_model(self): - self.loaded_model = tf.keras.models.load_model('resnet50_saved_model') \ No newline at end of file + self.loaded_model = tf.keras.models.load_model('resnet50_saved_model') + +### TF-TRT INT8 model +# Creating TF-TRT INT8 model requires a small calibration dataset. This data set ideally should represent the test data in production well, and will be used +# to create a value histogram for each layer in the neural network for effective 8-bit quantization. + +from __future__ import absolute_import, division, print_function, unicode_literals +import os +import time + +import numpy as np +import matplotlib.pyplot as plt + +import tensorflow as tf +from tensorflow import keras +from tensorflow.python.compiler.tensorrt import trt_convert as trt +from tensorflow.python.saved_model import tag_constants +from tensorflow.keras.applications.resnet50 import ResNet50 +from tensorflow.keras.preprocessing import image +from tensorflow.keras.applications.resnet50 import preprocess_input, decode_predictions + + +batch_size = 8 +batched_input = np.zeros((batch_size, 224, 224, 3), dtype=np.float32) + +for i in range(batch_size): + img_path = './data/img%d.JPG' % (i % 4) + img = image.load_img(img_path, target_size=(224, 224)) + x = image.img_to_array(img) + x = np.expand_dims(x, axis=0) + x = preprocess_input(x) + batched_input[i, :] = x +batched_input = tf.constant(batched_input) +print('batched_input shape: ', batched_input.shape) + +def calibration_input_fn(): + yield (batched_input, ) + +print('Converting to TF-TRT INT8...') + +converter = trt.TrtGraphConverterV2(input_saved_model_dir='resnet50_saved_model', + precision_mode=trt.TrtPrecisionMode.INT8, + max_workspace_size_bytes=8000000000) + +converter.convert(calibration_input_fn=calibration_input_fn) +converter.save(output_saved_model_dir='resnet50_saved_model_TFTRT_INT8') +print('Done Converting to TF-TRT INT8') + +def predict_tftrt(input_saved_model): + """Runs prediction on a single image and shows the result. + input_saved_model (string): Name of the input model stored in the current dir + """ + img_path = './data/img0.JPG' # Siberian_husky + img = image.load_img(img_path, target_size=(224, 224)) + x = image.img_to_array(img) + x = np.expand_dims(x, axis=0) + x = preprocess_input(x) + x = tf.constant(x) + + saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) + signature_keys = list(saved_model_loaded.signatures.keys()) + print(signature_keys) + + infer = saved_model_loaded.signatures['serving_default'] + print(infer.structured_outputs) + + labeling = infer(x) + preds = labeling['predictions'].numpy() + print('{} - Predicted: {}'.format(img_path, decode_predictions(preds, top=3)[0])) + plt.subplot(2,2,1) + plt.imshow(img); + plt.axis('off'); + plt.title(decode_predictions(preds, top=3)[0][0][1]) + +def benchmark_tftrt(input_saved_model): + saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) + infer = saved_model_loaded.signatures['serving_default'] + + N_warmup_run = 50 + N_run = 1000 + elapsed_time = [] + + for i in range(N_warmup_run): + labeling = infer(batched_input) + + for i in range(N_run): + start_time = time.time() + labeling = infer(batched_input) + #prob = labeling['probs'].numpy() + end_time = time.time() + elapsed_time = np.append(elapsed_time, end_time - start_time) + if i % 50 == 0: + print('Step {}: {:4.1f}ms'.format(i, (elapsed_time[-50:].mean()) * 1000)) + + print('Throughput: {:.0f} images/s'.format(N_run * batch_size / elapsed_time.sum())) + +# calibration: +# from tensorflow.python.compiler.tensorrt import trt_convert as trt +# Instantiate the TF-TRT converter +# converter = trt.TrtGraphConverterV2( +# input_saved_model_dir=SAVED_MODEL_DIR, +# precision_mode=trt.TrtPrecisionMode.INT8, +# use_calibration=True +# ) + +# # Use data from the test/validation set to perform INT8 calibration +# BATCH_SIZE=32 +# NUM_CALIB_BATCHES=10 +# def calibration_input_fn(): +# for i in range(NUM_CALIB_BATCHES): +# start_idx = i * BATCH_SIZE +# end_idx = (i + 1) * BATCH_SIZE +# x = x_test[start_idx:end_idx, :] +# yield [x] + +# # Convert the model and build the engine with valid calibration data +# func = converter.convert(calibration_input_fn=calibration_input_fn) +# converter.build(calibration_input_fn) + +# OUTPUT_SAVED_MODEL_DIR="./models/tftrt_saved_model" +# converter.save(output_saved_model_dir=OUTPUT_SAVED_MODEL_DIR) + +# converter.summary() + +# # Run some inferences! +# for step in range(10): +# start_idx = step * BATCH_SIZE +# end_idx = (step + 1) * BATCH_SIZE + +# print(f"Step: {step}") +# x = x_test[start_idx:end_idx, :] +# func(x) + +# https://github.com/tensorflow/tensorrt/blob/master/tftrt/examples-py/PTQ_example.ipynb +model = tf.keras.models.load_model('resnet50_saved_model') + +batch_size = 8 +batched_input = np.zeros((batch_size, 224, 224, 3), dtype=np.float32) + +for i in range(batch_size): + img_path = './data/img%d.JPG' % (i % 4) + img = image.load_img(img_path, target_size=(224, 224)) + x = image.img_to_array(img) + x = np.expand_dims(x, axis=0) + x = preprocess_input(x) + batched_input[i, :] = x +batched_input = tf.constant(batched_input) +print('batched_input shape: ', batched_input.shape) + +# Benchmarking throughput +N_warmup_run = 50 +N_run = 1000 +elapsed_time = [] + +for i in range(N_warmup_run): + preds = model.predict(batched_input) + +for i in range(N_run): + start_time = time.time() + preds = model.predict(batched_input) + end_time = time.time() + elapsed_time = np.append(elapsed_time, end_time - start_time) + if i % 50 == 0: + print('Step {}: {:4.1f}ms'.format(i, (elapsed_time[-50:].mean()) * 1000)) + +print('Throughput: {:.0f} images/s'.format(N_run * batch_size / elapsed_time.sum())) + +def predict_tftrt(input_saved_model): + """Runs prediction on a single image and shows the result. + input_saved_model (string): Name of the input model stored in the current dir + """ + img_path = './data/img0.JPG' # Siberian_husky + img = image.load_img(img_path, target_size=(224, 224)) + x = image.img_to_array(img) + x = np.expand_dims(x, axis=0) + x = preprocess_input(x) + x = tf.constant(x) + + saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) + signature_keys = list(saved_model_loaded.signatures.keys()) + print(signature_keys) + + infer = saved_model_loaded.signatures['serving_default'] + print(infer.structured_outputs) + + labeling = infer(x) + preds = labeling['predictions'].numpy() + print('{} - Predicted: {}'.format(img_path, decode_predictions(preds, top=3)[0])) + plt.subplot(2,2,1) + plt.imshow(img); + plt.axis('off'); + plt.title(decode_predictions(preds, top=3)[0][0][1]) + +def benchmark_tftrt(input_saved_model): + saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) + infer = saved_model_loaded.signatures['serving_default'] + + N_warmup_run = 50 + N_run = 1000 + elapsed_time = [] + + for i in range(N_warmup_run): + labeling = infer(batched_input) + + for i in range(N_run): + start_time = time.time() + labeling = infer(batched_input) + end_time = time.time() + elapsed_time = np.append(elapsed_time, end_time - start_time) + if i % 50 == 0: + print('Step {}: {:4.1f}ms'.format(i, (elapsed_time[-50:].mean()) * 1000)) + + print('Throughput: {:.0f} images/s'.format(N_run * batch_size / elapsed_time.sum())) \ No newline at end of file diff --git a/python_wip/TRT Quantization Examples.py b/python_wip/TRT Quantization Examples.py deleted file mode 100644 index 5f3c9de..0000000 --- a/python_wip/TRT Quantization Examples.py +++ /dev/null @@ -1,131 +0,0 @@ -### TF-TRT INT8 model - -# Creating TF-TRT INT8 model requires a small calibration dataset. This data set ideally should represent the test data in production well, and will be used -# to create a value histogram for each layer in the neural network for effective 8-bit quantization. - -from __future__ import absolute_import, division, print_function, unicode_literals -import os -import time - -import numpy as np -import matplotlib.pyplot as plt - -import tensorflow as tf -from tensorflow import keras -from tensorflow.python.compiler.tensorrt import trt_convert as trt -from tensorflow.python.saved_model import tag_constants -from tensorflow.keras.applications.resnet50 import ResNet50 -from tensorflow.keras.preprocessing import image -from tensorflow.keras.applications.resnet50 import preprocess_input, decode_predictions - - -batch_size = 8 -batched_input = np.zeros((batch_size, 224, 224, 3), dtype=np.float32) - -for i in range(batch_size): - img_path = './data/img%d.JPG' % (i % 4) - img = image.load_img(img_path, target_size=(224, 224)) - x = image.img_to_array(img) - x = np.expand_dims(x, axis=0) - x = preprocess_input(x) - batched_input[i, :] = x -batched_input = tf.constant(batched_input) -print('batched_input shape: ', batched_input.shape) - -def calibration_input_fn(): - yield (batched_input, ) - -print('Converting to TF-TRT INT8...') - -converter = trt.TrtGraphConverterV2(input_saved_model_dir='resnet50_saved_model', - precision_mode=trt.TrtPrecisionMode.INT8, - max_workspace_size_bytes=8000000000) - -converter.convert(calibration_input_fn=calibration_input_fn) -converter.save(output_saved_model_dir='resnet50_saved_model_TFTRT_INT8') -print('Done Converting to TF-TRT INT8') - -def predict_tftrt(input_saved_model): - """Runs prediction on a single image and shows the result. - input_saved_model (string): Name of the input model stored in the current dir - """ - img_path = './data/img0.JPG' # Siberian_husky - img = image.load_img(img_path, target_size=(224, 224)) - x = image.img_to_array(img) - x = np.expand_dims(x, axis=0) - x = preprocess_input(x) - x = tf.constant(x) - - saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) - signature_keys = list(saved_model_loaded.signatures.keys()) - print(signature_keys) - - infer = saved_model_loaded.signatures['serving_default'] - print(infer.structured_outputs) - - labeling = infer(x) - preds = labeling['predictions'].numpy() - print('{} - Predicted: {}'.format(img_path, decode_predictions(preds, top=3)[0])) - plt.subplot(2,2,1) - plt.imshow(img); - plt.axis('off'); - plt.title(decode_predictions(preds, top=3)[0][0][1]) - -def benchmark_tftrt(input_saved_model): - saved_model_loaded = tf.saved_model.load(input_saved_model, tags=[tag_constants.SERVING]) - infer = saved_model_loaded.signatures['serving_default'] - - N_warmup_run = 50 - N_run = 1000 - elapsed_time = [] - - for i in range(N_warmup_run): - labeling = infer(batched_input) - - for i in range(N_run): - start_time = time.time() - labeling = infer(batched_input) - #prob = labeling['probs'].numpy() - end_time = time.time() - elapsed_time = np.append(elapsed_time, end_time - start_time) - if i % 50 == 0: - print('Step {}: {:4.1f}ms'.format(i, (elapsed_time[-50:].mean()) * 1000)) - - print('Throughput: {:.0f} images/s'.format(N_run * batch_size / elapsed_time.sum())) - -# calibration: -# from tensorflow.python.compiler.tensorrt import trt_convert as trt -# Instantiate the TF-TRT converter -# converter = trt.TrtGraphConverterV2( -# input_saved_model_dir=SAVED_MODEL_DIR, -# precision_mode=trt.TrtPrecisionMode.INT8, -# use_calibration=True -# ) - -# # Use data from the test/validation set to perform INT8 calibration -# BATCH_SIZE=32 -# NUM_CALIB_BATCHES=10 -# def calibration_input_fn(): -# for i in range(NUM_CALIB_BATCHES): -# start_idx = i * BATCH_SIZE -# end_idx = (i + 1) * BATCH_SIZE -# x = x_test[start_idx:end_idx, :] -# yield [x] - -# # Convert the model and build the engine with valid calibration data -# func = converter.convert(calibration_input_fn=calibration_input_fn) -# converter.build(calibration_input_fn) - -# OUTPUT_SAVED_MODEL_DIR="./models/tftrt_saved_model" -# converter.save(output_saved_model_dir=OUTPUT_SAVED_MODEL_DIR) - -# converter.summary() - -# # Run some inferences! -# for step in range(10): -# start_idx = step * BATCH_SIZE -# end_idx = (step + 1) * BATCH_SIZE - -# print(f"Step: {step}") -# x = x_test[start_idx:end_idx, :] -# func(x) \ No newline at end of file diff --git a/python_wip/async_cuda.py b/python_wip/async_cuda.py new file mode 100644 index 0000000..a91f366 --- /dev/null +++ b/python_wip/async_cuda.py @@ -0,0 +1,100 @@ +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import Image +import cv2 +import numpy as np +import asyncio # Import asyncio for async handling +from trt_inference import TRTInference # TensorRT inference class +import threading # For concurrency control +import pycuda.driver as cuda # Assuming CUDA is required for inference + +class BoundingBoxNode(Node): + def __init__(self): + super().__init__('bounding_box_node') + + # Create reentrant callback groups for left and right image callbacks + self.callback_group_left = ReentrantCallbackGroup() + self.callback_group_right = ReentrantCallbackGroup() + + # Subscribers for left and right camera image topics, each with a different callback group + self.left_image_subscriber = self.create_subscription( + Image, '/camera/left/image_raw', self.left_image_callback, 10, callback_group=self.callback_group_left) + + self.right_image_subscriber = self.create_subscription( + Image, '/camera/right/image_raw', self.right_image_callback, 10, callback_group=self.callback_group_right) + + # Initialize TensorRT inference engine + self.trt_inference = TRTInference('model.engine') + self.cuda_stream = cuda.Stream() + + # Buffer for left and right images (shared resource) + self.left_image = None + self.right_image = None + + # Add a lock to avoid race conditions on image access + self.lock = threading.Lock() + + def left_image_callback(self, msg): + frame = self.ros2_image_to_numpy(msg) + with self.lock: + self.left_image = frame + + # If both left and right images are available, start async inference + if self.right_image is not None: + asyncio.create_task(self.run_inference()) + + def right_image_callback(self, msg): + frame = self.ros2_image_to_numpy(msg) + with self.lock: + self.right_image = frame + + # If both left and right images are available, start async inference + if self.left_image is not None: + asyncio.create_task(self.run_inference()) + + async def run_inference(self): + # Lock the shared resource (images) + with self.lock: + # Batch the images for inference + left_img = self.left_image + right_img = self.right_image + + # Reset images to avoid reprocessing the same images + self.left_image = None + self.right_image = None + + # Run inference asynchronously + if left_img is not None and right_img is not None: + await self.async_inference(left_img, right_img) + + async def async_inference(self, left_img, right_img): + # This method would use the CUDA stream to run TensorRT inference asynchronously. + # Actual TensorRT inference code goes here... + # Ensure that TensorRT uses the self.cuda_stream for asynchronous execution. + ... + + def ros2_image_to_numpy(self, msg): + img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return img + +def main(args=None): + rclpy.init(args=args) + + node = BoundingBoxNode() + + # Use a MultiThreadedExecutor with 2 threads + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/python_wip/composable_arch.py b/python_wip/composable_arch.py deleted file mode 100644 index a2ddbe7..0000000 --- a/python_wip/composable_arch.py +++ /dev/null @@ -1,27 +0,0 @@ -import rclpy -from rclpy.node import Node -from std_msgs.msg import String - -class ComposablePublisherNode(Node): - def __init__(self): - super().__init__('composable_publisher_node') - self.publisher_ = self.create_publisher(String, 'topic', 10) - timer_period = 2.0 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.get_logger().info("ComposablePublisherNode has been started.") - - def timer_callback(self): - msg = String() - msg.data = 'Hello from Composable Node!' - self.publisher_.publish(msg) - self.get_logger().info('Publishing: "%s"' % msg.data) - -def main(args=None): - rclpy.init(args=args) - node = ComposablePublisherNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/python_wip/composable_arch_new.py b/python_wip/composable_arch_new.py deleted file mode 100644 index 02eb45e..0000000 --- a/python_wip/composable_arch_new.py +++ /dev/null @@ -1,38 +0,0 @@ -import rclpy -from rclpy.node import Node -from std_msgs.msg import String - -class ComposablePublisherNode(Node): - def __init__(self): - super().__init__('composable_publisher_node') - - # Declare a parameter and use it in the node - self.declare_parameter('message', 'Hello from Composable Node!') - self.declare_parameter('publish_frequency', 2.0) - - self.publisher_ = self.create_publisher(String, 'topic', 10) - - # Fetch the parameters - self.message = self.get_parameter('message').get_parameter_value().string_value - self.publish_frequency = self.get_parameter('publish_frequency').get_parameter_value().double_value - - self.timer = self.create_timer(self.publish_frequency, self.timer_callback) - self.get_logger().info("ComposablePublisherNode has been started with message: '%s'" % self.message) - - def timer_callback(self): - msg = String() - msg.data = self.message - self.publisher_.publish(msg) - self.get_logger().info('Publishing: "%s"' % msg.data) - -def main(args=None): - rclpy.init(args=args) - node = ComposablePublisherNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() - -# allows parameters \ No newline at end of file diff --git a/python_wip/composable_node_launch.py b/python_wip/composable_node_launch.py deleted file mode 100644 index 8b239e0..0000000 --- a/python_wip/composable_node_launch.py +++ /dev/null @@ -1,29 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -def generate_launch_description(): - return LaunchDescription([ - ComposableNodeContainer( - name='camera_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='your_package_name', - plugin='your_package_name.CameraNode', - name='camera_node', - parameters=[{ - 'source_type': 'zed', - 'static_image_path': '.../assets/', - 'video_path': '.../assets/video.mp4', - 'loop': 0, - 'frame_rate': 30, - 'model_type': 'maize', - }], - ), - ], - output='screen', - ), - ]) diff --git a/python_wip/cuda_nonblocking.py b/python_wip/cuda_nonblocking.py new file mode 100644 index 0000000..f3264ed --- /dev/null +++ b/python_wip/cuda_nonblocking.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +import pycuda.driver as cuda +import numpy as np +import cv2 +import threading + +class ImagePreprocessingNode(Node): + def __init__(self, sync_event): + super().__init__('image_preprocessing_node') + self.sync_event = sync_event # Synchronization event to signal inference node + self.width = 224 + self.height = 224 + self.channels = 3 + + # Allocate CUDA memory for the image and create CUDA stream for non-blocking processing + self.cuda_mem = cuda.mem_alloc(self.width * self.height * self.channels * np.dtype(np.float32).itemsize) + self.stream = cuda.Stream() # Create CUDA stream for asynchronous execution + + # Create a subscription to image topic (e.g., from a camera) + self.subscription = self.create_subscription( + Image, + '/camera/image', + self.image_callback, + 10 + ) + + def image_callback(self, msg): + # Convert ROS Image message to a numpy array (assuming the image is encoded in RGB8) + image = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, self.channels) + image = cv2.resize(image, (self.width, self.height)).astype(np.float32) / 255.0 + + # Copy image from host (CPU) to device (GPU) asynchronously using CUDA stream + cuda.memcpy_htod_async(self.cuda_mem, image, self.stream) + + # After copying, the image is preprocessed asynchronously, use CUDA stream to synchronize + self.stream.synchronize() # Wait for the preprocessing task to complete + + # Signal the inference node that preprocessing is done + self.sync_event.set() + self.get_logger().info("Image preprocessed and copied to CUDA memory") + + def get_cuda_memory(self): + return self.cuda_mem + + def get_cuda_stream(self): + return self.stream # Expose the CUDA stream for the inference node to use diff --git a/python_wip/cuda_nonblocking_infer.py b/python_wip/cuda_nonblocking_infer.py new file mode 100644 index 0000000..274368b --- /dev/null +++ b/python_wip/cuda_nonblocking_infer.py @@ -0,0 +1,55 @@ +import rclpy +from rclpy.node import Node +import tensorrt as trt +import pycuda.driver as cuda +import numpy as np +import threading + +class InferenceNode(Node): + def __init__(self, preprocess_node, sync_event): + super().__init__('inference_node') + self.preprocess_node = preprocess_node + self.sync_event = sync_event # Synchronization event + self.create_timer(0.1, self.run_inference) # Periodically check for new images + + # Load the TensorRT engine + self.engine = self.load_engine('model.trt') + self.context = self.engine.create_execution_context() + + # Allocate device memory for input/output bindings + self.input_shape = (1, 3, 224, 224) + self.input_size = np.prod(self.input_shape) * np.dtype(np.float32).itemsize + self.d_output = cuda.mem_alloc(self.input_size) # assuming output size is the same as input + + # Create a CUDA stream for asynchronous inference execution + self.inference_stream = cuda.Stream() + + def load_engine(self, engine_path): + with open(engine_path, "rb") as f: + runtime = trt.Runtime(trt.Logger(trt.Logger.WARNING)) + return runtime.deserialize_cuda_engine(f.read()) + + def run_inference(self): + # Wait until the preprocessing node signals that a new image is ready + if not self.sync_event.is_set(): + return # No image ready for inference yet + + # Reset the event so that the preprocessing node can signal for the next image + self.sync_event.clear() + + # Get the CUDA memory pointer and stream from the preprocessing node + d_preprocessed_image = self.preprocess_node.get_cuda_memory() + preprocess_stream = self.preprocess_node.get_cuda_stream() + + # Run inference using TensorRT asynchronously in its own stream + bindings = [int(d_preprocessed_image), int(self.d_output)] + self.context.execute_async_v2(bindings, self.inference_stream.handle) + + # Synchronize the inference stream to ensure inference is complete before proceeding + self.inference_stream.synchronize() + + # Assuming the result is in d_output, we could copy it back to host if needed + result = np.empty(self.input_shape, dtype=np.float32) + cuda.memcpy_dtoh_async(result, self.d_output, self.inference_stream) + self.inference_stream.synchronize() # Ensure the copy operation is done + self.get_logger().info(f"Inference complete. Output shape: {result.shape}") diff --git a/python_wip/cuda_sync.py b/python_wip/cuda_sync.py new file mode 100644 index 0000000..c093c3e --- /dev/null +++ b/python_wip/cuda_sync.py @@ -0,0 +1,41 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +import pycuda.driver as cuda +import numpy as np +import cv2 +import threading + +class ImagePreprocessingNode(Node): + def __init__(self, sync_event): + super().__init__('image_preprocessing_node') + self.sync_event = sync_event # Synchronization event to signal inference node + self.width = 224 + self.height = 224 + self.channels = 3 + + # Allocate CUDA memory for the image + self.cuda_mem = cuda.mem_alloc(self.width * self.height * self.channels * np.dtype(np.float32).itemsize) + + # Create a subscription to image topic (e.g., from a camera) + self.subscription = self.create_subscription( + Image, + '/camera/image', + self.image_callback, + 10 + ) + + def image_callback(self, msg): + # Convert ROS Image message to a numpy array (assuming the image is encoded in RGB8) + image = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, self.channels) + image = cv2.resize(image, (self.width, self.height)).astype(np.float32) / 255.0 + + # Copy image from host (CPU) to device (GPU) + cuda.memcpy_htod(self.cuda_mem, image) + + # Signal the inference node that preprocessing is done + self.sync_event.set() + self.get_logger().info("Image preprocessed and copied to CUDA memory") + + def get_cuda_memory(self): + return self.cuda_mem diff --git a/python_wip/cuda_sync_infer.py b/python_wip/cuda_sync_infer.py new file mode 100644 index 0000000..6f0fa70 --- /dev/null +++ b/python_wip/cuda_sync_infer.py @@ -0,0 +1,47 @@ +import rclpy +from rclpy.node import Node +import tensorrt as trt +import pycuda.driver as cuda +import numpy as np +import threading + +class InferenceNode(Node): + def __init__(self, preprocess_node, sync_event): + super().__init__('inference_node') + self.preprocess_node = preprocess_node + self.sync_event = sync_event # Synchronization event + self.create_timer(0.1, self.run_inference) # Periodically check for new images + + # Load the TensorRT engine + self.engine = self.load_engine('model.trt') + self.context = self.engine.create_execution_context() + + # Allocate device memory for input/output bindings + self.input_shape = (1, 3, 224, 224) + self.input_size = np.prod(self.input_shape) * np.dtype(np.float32).itemsize + self.d_output = cuda.mem_alloc(self.input_size) # assuming output size is the same as input + + def load_engine(self, engine_path): + with open(engine_path, "rb") as f: + runtime = trt.Runtime(trt.Logger(trt.Logger.WARNING)) + return runtime.deserialize_cuda_engine(f.read()) + + def run_inference(self): + # Wait until the preprocessing node signals that a new image is ready + if not self.sync_event.is_set(): + return # No image ready for inference yet + + # Reset the event so that the preprocessing node can signal for the next image + self.sync_event.clear() + + # Get the CUDA memory pointer from the preprocessing node + d_preprocessed_image = self.preprocess_node.get_cuda_memory() + + # Run inference using TensorRT on the preprocessed image + bindings = [int(d_preprocessed_image), int(self.d_output)] + self.context.execute_v2(bindings) + + # Assuming the result is in d_output, we could copy it back to host if needed + result = np.empty(self.input_shape, dtype=np.float32) + cuda.memcpy_dtoh(result, self.d_output) + self.get_logger().info(f"Inference complete. Output shape: {result.shape}") diff --git a/python_wip/cuda_sync_launch.py b/python_wip/cuda_sync_launch.py new file mode 100644 index 0000000..89cd29f --- /dev/null +++ b/python_wip/cuda_sync_launch.py @@ -0,0 +1,41 @@ +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch import LaunchDescription +import threading + +sync_event = threading.Event() + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='gpu_composable_container', + namespace='', + package='rclpy_components', + executable='component_container_mt', # Multi-threaded container for composable nodes + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='my_package.ImagePreprocessingNode', + name='image_preprocessing_node', + parameters=[ + {"sync_event": sync_event} + ] + ), + ComposableNode( + package='my_package', + plugin='my_package.InferenceNode', + name='inference_node', + parameters=[ + {"preprocess_node": "image_preprocessing_node", "sync_event": sync_event} + ] + ), + ], + output='screen', + ), + ]) + +# Threading Event: The synchronization between the preprocessing and inference nodes is handled by a threading event (sync_event). The event is set by the image preprocessing node once the image is copied to CUDA memory and is ready for inference. The inference node waits for this event to be set, indicating that the preprocessing is done, and clears the event after starting inference. This ensures that each image is processed in order and no data is overwritten or lost. + +# Callback-Based Image Processing: The image preprocessing node processes each incoming image in the callback from the ROS2 image subscription. After processing the image, it signals the inference node to begin the inference process. + +# Periodic Inference Check: The inference node runs on a timer that periodically checks whether a new image is ready by checking the threading event. Once the event is set (indicating that an image is ready), it retrieves the CUDA memory pointer and performs inference. \ No newline at end of file diff --git a/python_wip/idk1.py b/python_wip/idk1.py deleted file mode 100644 index 4e4879d..0000000 --- a/python_wip/idk1.py +++ /dev/null @@ -1,56 +0,0 @@ -import timeit, os -import tensorrt as trt -import pycuda.driver as cuda -import pycuda.autoinit -from decord import VideoReader, cpu, gpu -import numpy as np -import torch -from torchvision.transforms import Normalize - -BATCH_SIZE = 8 # try 2, 4, 16 -target_dtype = np.float16 - -os.chdir('/home/user/AppliedAI/23-I-12_SysArch/Experiments/ishaan_workspace/src/pipeline/node_test/node_test') -f = open("resnet_engine_pytorch.trt", "rb") -runtime = trt.Runtime(trt.Logger(trt.Logger.WARNING)) -engine = runtime.deserialize_cuda_engine(f.read()) -context = engine.create_execution_context() - -global frames -# We can utilize the libavfilter library to do frame resize for us -vr = VideoReader('City.mp4', ctx=gpu(0), width=640, height=480) # cpu while on same process? -print('video frames:', len(vr)) -for i in range(len(vr)/BATCH_SIZE): - frames = vr.get_batch(range(i*BATCH_SIZE, (i+1)*BATCH_SIZE)).asnumpy() - print(frames.shape) - print(frames.dtype) - print(frames.context) - - timeit.timeit(lambda: preprocess_image(), number=1, globals=globals()) - - timeit.timeit(lambda: predict(), number=1, globals=globals()) - -# allocate device memory -d_input = cuda.mem_alloc(1 * frames.nbytes) -output = np.empty([BATCH_SIZE, 1000], dtype = target_dtype) -d_output = cuda.mem_alloc(1 * output.nbytes) -bindings = [int(d_input), int(d_output)] -stream = cuda.Stream() - -def preprocess_image(): - global frames - norm = Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) - frames = norm(torch.from_numpy(frames).transpose(0,2).transpose(1,2)) - frames.astype(np.float16, copy=False) - # preprocessed_images = np.array([preprocess_image(image) for image in input_batch]) - -def predict(batch): # result gets copied into output - # transfer input data to device - cuda.memcpy_htod_async(d_input, batch, stream) - # execute model - context.execute_async_v2(bindings, stream.handle, None) - # transfer predictions back - cuda.memcpy_dtoh_async(output, d_output, stream) - # syncronize threads - stream.synchronize() - return output \ No newline at end of file diff --git a/python_wip/idk2.py b/python_wip/idk2.py deleted file mode 100644 index b135b1c..0000000 --- a/python_wip/idk2.py +++ /dev/null @@ -1,69 +0,0 @@ -def allocate_buffers(engine, batch_size, data_type): - - """ - This is the function to allocate buffers for input and output in the device - Args: - engine : The path to the TensorRT engine. - batch_size : The batch size for execution time. - data_type: The type of the data for input and output, for example trt.float32. - - Output: - h_input_1: Input in the host. - d_input_1: Input in the device. - h_output_1: Output in the host. - d_output_1: Output in the device. - stream: CUDA stream. - """ - - # Determine dimensions and create page-locked memory buffers (which won't be swapped to disk) to hold host inputs/outputs. - h_input_1 = cuda.pagelocked_empty(batch_size * trt.volume(engine.get_binding_shape(0)), dtype=trt.nptype(data_type)) - h_output = cuda.pagelocked_empty(batch_size * trt.volume(engine.get_binding_shape(1)), dtype=trt.nptype(data_type)) - # Allocate device memory for inputs and outputs. - d_input_1 = cuda.mem_alloc(h_input_1.nbytes) - - d_output = cuda.mem_alloc(h_output.nbytes) - # Create a stream in which to copy inputs/outputs and run inference. - stream = cuda.Stream() - return h_input_1, d_input_1, h_output, d_output, stream - -def load_images_to_buffer(pics, pagelocked_buffer): - preprocessed = np.asarray(pics).ravel() - np.copyto(pagelocked_buffer, preprocessed) - -def do_inference(engine, pics_1, h_input_1, d_input_1, h_output, d_output, stream, batch_size, height, width): - """ - This is the function to run the inference - Args: - engine : Path to the TensorRT engine - pics_1 : Input images to the model. - h_input_1: Input in the host - d_input_1: Input in the device - h_output_1: Output in the host - d_output_1: Output in the device - stream: CUDA stream - batch_size : Batch size for execution time - height: Height of the output image - width: Width of the output image - - Output: - The list of output images - """ - - load_images_to_buffer(pics_1, h_input_1) - - with engine.create_execution_context() as context: - # Transfer input data to the GPU. - cuda.memcpy_htod_async(d_input_1, h_input_1, stream) - - # Run inference. - - context.profiler = trt.Profiler() - context.execute(batch_size=1, bindings=[int(d_input_1), int(d_output)]) - - # Transfer predictions back from the GPU. - cuda.memcpy_dtoh_async(h_output, d_output, stream) - # Synchronize the stream - stream.synchronize() - # Return the host output. - out = h_output.reshape((batch_size,-1, height, width)) - return out \ No newline at end of file diff --git a/python_wip/lifecycle_arch.py b/python_wip/lifecycle_arch.py deleted file mode 100644 index 02c748c..0000000 --- a/python_wip/lifecycle_arch.py +++ /dev/null @@ -1,50 +0,0 @@ -# lifecycle_node.py -import rclpy -from rclpy.lifecycle import LifecycleNode -from rclpy.lifecycle import State -from rclpy.lifecycle import TransitionCallbackReturn -from rclpy.executors import MultiThreadedExecutor - -class LifecycleManagerNode(LifecycleNode): - def __init__(self): - super().__init__('lifecycle_manager_node') - self.get_logger().info('LifecycleManagerNode has been created.') - - def on_configure(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_configure() called') - return TransitionCallbackReturn.SUCCESS - - def on_activate(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_activate() called') - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_deactivate() called') - return TransitionCallbackReturn.SUCCESS - - def on_cleanup(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_cleanup() called') - return TransitionCallbackReturn.SUCCESS - - def on_shutdown(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_shutdown() called') - return TransitionCallbackReturn.SUCCESS - -def main(args=None): - rclpy.init(args=args) - - lifecycle_node = LifecycleManagerNode() - - executor = MultiThreadedExecutor() - executor.add_node(lifecycle_node) - - try: - rclpy.spin(lifecycle_node, executor=executor) - except KeyboardInterrupt: - pass - - lifecycle_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/python_wip/lifecycle_arch_new.py b/python_wip/lifecycle_arch_new.py deleted file mode 100644 index 9c894e0..0000000 --- a/python_wip/lifecycle_arch_new.py +++ /dev/null @@ -1,55 +0,0 @@ -# lifecycle_node.py -import rclpy -from rclpy.lifecycle import LifecycleNode -from rclpy.lifecycle import State -from rclpy.lifecycle import TransitionCallbackReturn -from rclpy.executors import MultiThreadedExecutor - -class LifecycleManagerNode(LifecycleNode): - def __init__(self): - super().__init__('lifecycle_manager_node') - - # Declare parameters for the lifecycle node - self.declare_parameter('lifecycle_action', 'activate') - self.get_logger().info('LifecycleManagerNode has been created.') - - def on_configure(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_configure() called') - return TransitionCallbackReturn.SUCCESS - - def on_activate(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_activate() called') - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_deactivate() called') - return TransitionCallbackReturn.SUCCESS - - def on_cleanup(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_cleanup() called') - return TransitionCallbackReturn.SUCCESS - - def on_shutdown(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info('on_shutdown() called') - return TransitionCallbackReturn.SUCCESS - -def main(args=None): - rclpy.init(args=args) - - lifecycle_node = LifecycleManagerNode() - - executor = MultiThreadedExecutor() - executor.add_node(lifecycle_node) - - try: - rclpy.spin(lifecycle_node, executor=executor) - except KeyboardInterrupt: - pass - - lifecycle_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() - -# parameters \ No newline at end of file diff --git a/python_wip/lifecycle_idkold.py b/python_wip/lifecycle_architecture.py similarity index 72% rename from python_wip/lifecycle_idkold.py rename to python_wip/lifecycle_architecture.py index 044c87c..d023499 100644 --- a/python_wip/lifecycle_idkold.py +++ b/python_wip/lifecycle_architecture.py @@ -1,3 +1,57 @@ +# lifecycle_node.py +import rclpy +from rclpy.lifecycle import LifecycleNode +from rclpy.lifecycle import State +from rclpy.lifecycle import TransitionCallbackReturn +from rclpy.executors import MultiThreadedExecutor + +class LifecycleManagerNode(LifecycleNode): + def __init__(self): + super().__init__('lifecycle_manager_node') + + # Declare parameters for the lifecycle node + self.declare_parameter('lifecycle_action', 'activate') + self.get_logger().info('LifecycleManagerNode has been created.') + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_configure() called') + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_activate() called') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_deactivate() called') + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_cleanup() called') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_shutdown() called') + return TransitionCallbackReturn.SUCCESS + +def main(args=None): + rclpy.init(args=args) + + lifecycle_node = LifecycleManagerNode() + + executor = MultiThreadedExecutor() + executor.add_node(lifecycle_node) + + try: + rclpy.spin(lifecycle_node, executor=executor) + except KeyboardInterrupt: + pass + + lifecycle_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + import rclpy from rclpy.lifecycle import Node as LifecycleNode from rclpy.lifecycle import State diff --git a/python_wip/monolith_multhread.py b/python_wip/monolith_multhread.py new file mode 100644 index 0000000..8c7d39a --- /dev/null +++ b/python_wip/monolith_multhread.py @@ -0,0 +1,88 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +import cv2 +import numpy as np +import pycuda.driver as cuda +from threading import Thread +from trt_inference import TRTInference # Assume custom TensorRT wrapper + +class BoundingBoxNode(Node): + def __init__(self): + super().__init__('bounding_box_node') + + # Create subscriber to ZED camera image topic + self.image_subscriber = self.create_subscription(Image, '/zed/image_raw', self.image_callback, 10) + + # Load TensorRT engine + self.trt_inference = TRTInference('model.engine') # Custom class for TensorRT inference + self.cuda_stream = cuda.Stream() # Use an asynchronous CUDA stream + + # Initialize threading for preprocessing, inference, and post-processing + self.inference_thread = None + + def image_callback(self, msg): + # Convert ROS2 Image to a NumPy array (assume it's in BGR format) + frame = self.ros2_image_to_numpy(msg) + + # Asynchronous preprocessing and inference using a separate thread + if self.inference_thread is None or not self.inference_thread.is_alive(): + self.inference_thread = Thread(target=self.run_inference, args=(frame,)) + self.inference_thread.start() + + def run_inference(self, frame): + # Preprocessing: Resize, normalize, convert to TensorRT input format + preprocessed_frame = self.preprocess_image(frame) + + # Asynchronously copy input to the GPU + self.trt_inference.set_input_async(preprocessed_frame, stream=self.cuda_stream) + + # Run asynchronous inference + self.trt_inference.infer_async(stream=self.cuda_stream) + + # Asynchronously copy output from the GPU + output = self.trt_inference.get_output_async(stream=self.cuda_stream) + + # Post-process bounding boxes and other outputs (e.g., NMS) + bboxes = self.post_process(output) + + # Optionally publish bounding boxes or visualize results + self.publish_bounding_boxes(bboxes) + + def preprocess_image(self, frame): + """Perform preprocessing steps (resize, normalize, etc.) using CUDA or OpenCV.""" + # Example resizing and normalization using OpenCV (offloadable to GPU using Numba or custom CUDA kernels) + resized_frame = cv2.resize(frame, (self.trt_inference.input_width, self.trt_inference.input_height)) + normalized_frame = resized_frame.astype(np.float32) / 255.0 # Simple normalization + return normalized_frame + + def post_process(self, output): + """Post-process the TensorRT output to extract bounding boxes.""" + # Implement bounding box extraction, confidence thresholding, and optional NMS + bboxes = self.extract_bounding_boxes(output) + return bboxes + + def publish_bounding_boxes(self, bboxes): + """Convert bounding boxes to a suitable ROS2 message and publish (optional).""" + # You can add the code to publish the bounding boxes to a ROS2 topic here + pass + + def ros2_image_to_numpy(self, msg): + """Convert ROS2 Image message to a NumPy array (assumed to be in BGR format).""" + # Assuming the image is in BGR8 encoding + img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return img + +def main(args=None): + rclpy.init(args=args) + node = BoundingBoxNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/python_wip/overlap.py b/python_wip/overlap.py deleted file mode 100644 index 5ed94a7..0000000 --- a/python_wip/overlap.py +++ /dev/null @@ -1,30 +0,0 @@ -# Check overlap between bounding boxes -overlap_count = 0 -for bbox in self.current_bounding_boxes: - for filtered_bbox in filtered_bboxes: - if self.calculate_overlap(bbox, filtered_bbox) > 0.5: # Threshold of 50% overlap - overlap_count += 1 - break # Ensure each bounding box is only counted once - -def calculate_overlap(self, bbox1, bbox2): - x1_min, y1_min, x1_max, y1_max = bbox1.x_min, bbox1.y_min, bbox1.x_max, bbox1.y_max - x2_min, y2_min, x2_max, y2_max = bbox2 - - # Calculate intersection area - inter_x_min = max(x1_min, x2_min) - inter_y_min = max(y1_min, y2_min) - inter_x_max = min(x1_max, x2_max) - inter_y_max = min(y1_max, y2_max) - - inter_area = max(0, inter_x_max - inter_x_min) * max(0, inter_y_max - inter_y_min) - - # Calculate the area of both bounding boxes - bbox1_area = (x1_max - x1_min) * (y1_max - y1_min) - bbox2_area = (x2_max - x2_min) * (y2_max - y2_min) - - # Calculate the union area - union_area = bbox1_area + bbox2_area - inter_area - - # Calculate the Intersection over Union (IoU) - iou = inter_area / float(union_area) if union_area > 0 else 0 - return iou \ No newline at end of file diff --git a/python_wip/pinned_multithread.py b/python_wip/pinned_multithread.py new file mode 100644 index 0000000..962ea53 --- /dev/null +++ b/python_wip/pinned_multithread.py @@ -0,0 +1,129 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +import cv2 +import numpy as np +import pycuda.driver as cuda +import pycuda.autoinit +from threading import Thread +from trt_inference import TRTInference # Custom TensorRT wrapper + +class BoundingBoxNode(Node): + def __init__(self): + super().__init__('bounding_box_node') + + # Subscribers for left and right camera image topics + self.left_image_subscriber = self.create_subscription(Image, '/camera/left/image_raw', self.left_image_callback, 10) + self.right_image_subscriber = self.create_subscription(Image, '/camera/right/image_raw', self.right_image_callback, 10) + + # Initialize TensorRT inference engine + self.trt_inference = TRTInference('model.engine') # Custom class for TensorRT inference + self.cuda_stream = cuda.Stream() # Use an asynchronous CUDA stream + + # Buffer for left and right images + self.left_image = None + self.right_image = None + + # Allocate pinned memory (page-locked) for faster memory transfers + self.host_input = cuda.pagelocked_empty(shape=(2, self.trt_inference.input_height, self.trt_inference.input_width, 3), dtype=np.float32) # Batch of 2 images + + # Allocate memory for output + self.host_output = cuda.pagelocked_empty(shape=(self.trt_inference.output_size,), dtype=np.float32) + + # Initialize threading + self.inference_thread = None + + def left_image_callback(self, msg): + frame = self.ros2_image_to_numpy(msg) + self.left_image = frame + + # If both left and right images are available, start inference + if self.right_image is not None: + if self.inference_thread is None or not self.inference_thread.is_alive(): + self.inference_thread = Thread(target=self.run_inference) + self.inference_thread.start() + + def right_image_callback(self, msg): + frame = self.ros2_image_to_numpy(msg) + self.right_image = frame + + # If both left and right images are available, start inference + if self.left_image is not None: + if self.inference_thread is None or not self.inference_thread.is_alive(): + self.inference_thread = Thread(target=self.run_inference) + self.inference_thread.start() + + def run_inference(self): + # Batch left and right images + self.preprocess_image(self.left_image, 0) # Preprocess left image into batch slot 0 + self.preprocess_image(self.right_image, 1) # Preprocess right image into batch slot 1 + + # Asynchronously copy input batch to GPU + self.trt_inference.set_input_async(self.host_input, stream=self.cuda_stream) + + # Run asynchronous inference + self.trt_inference.infer_async(stream=self.cuda_stream) + + # Asynchronously copy output from GPU to host + output = self.trt_inference.get_output_async(self.host_output, stream=self.cuda_stream) + + # Post-process bounding boxes and other outputs (e.g., NMS) + bboxes = self.post_process(output) + + # Optionally publish bounding boxes or visualize results + self.publish_bounding_boxes(bboxes) + + # Reset left and right images for the next batch + self.left_image = None + self.right_image = None + + def preprocess_image(self, frame, index): + """Perform preprocessing steps (resize, normalize, etc.) and copy to the pinned memory.""" + resized_frame = cv2.resize(frame, (self.trt_inference.input_width, self.trt_inference.input_height)) + normalized_frame = resized_frame.astype(np.float32) / 255.0 # Simple normalization + + # Copy preprocessed image to the pinned memory buffer at the correct batch index + self.host_input[index] = normalized_frame + + def post_process(self, output): + """Post-process the TensorRT output to extract bounding boxes.""" + # Implement bounding box extraction, confidence thresholding, and optional NMS + bboxes = self.extract_bounding_boxes(output) + return bboxes + + def publish_bounding_boxes(self, bboxes): + """Convert bounding boxes to a suitable ROS2 message and publish (optional).""" + # You can add the code to publish the bounding boxes to a ROS2 topic here + pass + + def ros2_image_to_numpy(self, msg): + """Convert ROS2 Image message to a NumPy array (assumed to be in BGR format).""" + img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return img + +def main(args=None): + rclpy.init(args=args) + node = BoundingBoxNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + +# stream synchronization: +# Run asynchronous inference +self.trt_inference.infer_async(stream=self.cuda_stream) + +# Asynchronously copy output from GPU to host +output = self.trt_inference.get_output_async(self.host_output, stream=self.cuda_stream) + +# Synchronize stream to ensure all GPU operations are complete before proceeding +self.cuda_stream.synchronize() + +# Now it's safe to process the results +bboxes = self.post_process(output) diff --git a/python_wip/pinned_multithread_executor.py b/python_wip/pinned_multithread_executor.py new file mode 100644 index 0000000..1e15b02 --- /dev/null +++ b/python_wip/pinned_multithread_executor.py @@ -0,0 +1,214 @@ +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import Image +import cv2 +import numpy as np +from trt_inference import TRTInference # TensorRT inference class + +class BoundingBoxNode(Node): + def __init__(self): + super().__init__('bounding_box_node') + + # Create reentrant callback groups for left and right image callbacks + self.callback_group_left = ReentrantCallbackGroup() + self.callback_group_right = ReentrantCallbackGroup() + + # Subscribers for left and right camera image topics, each with a different callback group + self.left_image_subscriber = self.create_subscription( + Image, '/camera/left/image_raw', self.left_image_callback, 10, callback_group=self.callback_group_left) + + self.right_image_subscriber = self.create_subscription( + Image, '/camera/right/image_raw', self.right_image_callback, 10, callback_group=self.callback_group_right) + + # Initialize TensorRT inference engine + self.trt_inference = TRTInference('model.engine') + self.cuda_stream = cuda.Stream() + + # Buffer for left and right images + self.left_image = None + self.right_image = None + + def left_image_callback(self, msg): + frame = self.ros2_image_to_numpy(msg) + self.left_image = frame + + # If both left and right images are available, start inference + if self.right_image is not None: + self.run_inference() + + def right_image_callback(self, msg): + frame = self.ros2_image_to_numpy(msg) + self.right_image = frame + + # If both left and right images are available, start inference + if self.left_image is not None: + self.run_inference() + + def run_inference(self): + # Batch left and right images and run inference here + ... + + def ros2_image_to_numpy(self, msg): + img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return img + +def main(args=None): + rclpy.init(args=args) + + node = BoundingBoxNode() + + # Use a MultiThreadedExecutor with 2 threads + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + +# ReentrantCallbackGroup: This callback group allows the same callback to be executed in parallel across multiple threads. Each subscription (left and right image) is assigned a different ReentrantCallbackGroup to ensure they can be processed concurrently. + +# MultiThreadedExecutor: The MultiThreadedExecutor is created with a thread pool size of 2, allowing two callbacks (left and right image) to be processed in parallel. + +# parallelize preprocessing, inference, postprocessing, and publishing operations while ensuring proper GPU context and memory management, we can leverage ROS2's multithreaded executor with callback groups for different stages of the pipeline. + +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import Image +import cv2 +import numpy as np +import pycuda.driver as cuda + +from trt_inference import TRTInference # TensorRT inference class + +class BoundingBoxNode(Node): + def __init__(self): + super().__init__('bounding_box_node') + + # Initialize callback groups for parallel processing + self.preprocess_group = ReentrantCallbackGroup() + self.inference_group = ReentrantCallbackGroup() + self.postprocess_group = ReentrantCallbackGroup() + self.publish_group = ReentrantCallbackGroup() + + # Subscribers for left and right camera image topics, each in the preprocess group + self.left_image_subscriber = self.create_subscription( + Image, '/camera/left/image_raw', self.left_image_callback, 10, callback_group=self.preprocess_group) + + self.right_image_subscriber = self.create_subscription( + Image, '/camera/right/image_raw', self.right_image_callback, 10, callback_group=self.preprocess_group) + + # Initialize TensorRT inference engine + self.trt_inference = TRTInference('model.engine') + + # CUDA stream for asynchronous operations + self.cuda_stream = cuda.Stream() + + # Buffers for left and right images + self.left_image = None + self.right_image = None + + # Create publishers to publish bounding box results + self.bounding_boxes_publisher = self.create_publisher( + Image, '/bounding_boxes', 10, callback_group=self.publish_group) + + def left_image_callback(self, msg): + # Preprocess left image + self.get_logger().info('Preprocessing left image...') + frame = self.ros2_image_to_numpy(msg) + self.left_image = self.preprocess_image(frame) + + # If both left and right images are ready, run inference + if self.right_image is not None: + self.create_task(self.run_inference, self.inference_group) + + def right_image_callback(self, msg): + # Preprocess right image + self.get_logger().info('Preprocessing right image...') + frame = self.ros2_image_to_numpy(msg) + self.right_image = self.preprocess_image(frame) + + # If both left and right images are ready, run inference + if self.left_image is not None: + self.create_task(self.run_inference, self.inference_group) + + def preprocess_image(self, image): + # Example preprocessing: resize, normalize, etc. + # Perform any necessary transformations for the model + return cv2.resize(image, (224, 224)) + + def run_inference(self): + # Ensure both images are preprocessed before inference + if self.left_image is not None and self.right_image is not None: + self.get_logger().info('Running inference...') + + # Batch left and right images together + batched_images = np.stack([self.left_image, self.right_image]) + + # Run asynchronous inference on GPU using the CUDA stream + self.trt_inference.infer_async(batched_images, stream=self.cuda_stream) + + # Asynchronously copy the inference results from GPU to host + self.create_task(self.postprocess_results, self.postprocess_group) + + def postprocess_results(self): + self.get_logger().info('Postprocessing results...') + # Wait for inference and data transfers to complete + self.cuda_stream.synchronize() + + # Retrieve and postprocess the results + output = self.trt_inference.get_output() + bboxes = self.post_process(output) + + # Publish the results + self.create_task(lambda: self.publish_results(bboxes), self.publish_group) + + def post_process(self, output): + # Convert raw output into bounding boxes or detection results + return output # Example post-processing step + + def publish_results(self, bboxes): + self.get_logger().info('Publishing results...') + # Publish bounding boxes or any other output format + # Placeholder for publishing logic; replace with actual ROS message type + msg = Image() # Use the appropriate message type for bounding boxes + self.bounding_boxes_publisher.publish(msg) + + def ros2_image_to_numpy(self, msg): + # Convert ROS2 Image message to a NumPy array + return np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + + def create_task(self, target_function, callback_group): + # Helper function to execute tasks within the appropriate callback group + self.get_logger().info('Creating async task...') + self.executor.submit(target_function) + +def main(args=None): + rclpy.init(args=args) + + node = BoundingBoxNode() + + # Use a MultiThreadedExecutor with multiple threads for parallel callbacks + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/python_wip/rand_infer.py b/python_wip/rand_infer.py new file mode 100644 index 0000000..e696705 --- /dev/null +++ b/python_wip/rand_infer.py @@ -0,0 +1,78 @@ +# Load TensorRT engine and create execution context (example) +TRT_LOGGER = trt.Logger(trt.Logger.WARNING) + +def load_engine(engine_file_path): + with open(engine_file_path, "rb") as f, trt.Runtime(TRT_LOGGER) as runtime: + return runtime.deserialize_cuda_engine(f.read()) + +engine = load_engine("model.trt") +context = engine.create_execution_context() + +if self.engine_path.endswith('.trt') or self.engine_path.endswith('.engine'): + if self.strip_weights: + self.engine = self.load_stripped_engine_and_refit() + self.type = "trt_stripped" + else: + self.engine = self.load_normal_engine() + self.type = "trt_normal" +elif self.engine_path.endswith('.pth'): + from torch2trt import TRTModule + import torch + self.engine = TRTModule() + (self.engine).load_state_dict(torch.load(self.engine_path)) + self.type = "torch_trt" +else: + self.get_logger().error("Invalid engine file format. Please provide a .trt, .engine, or .pth file") + return None + +if self.type == "trt_stripped" or self.type == "trt_normal": + self.allocate_buffers() + self.exec_context = (self.engine).create_execution_context() +else: + self.inference_type = "torch_trt" + +def load_stripped_engine_and_refit(self): + if not os.path.exists(self.engine_path): + self.get_logger().error(f"Engine file not found at {self.engine_path}") + return None + + if not os.path.exists(self.model_path): + self.get_logger().error(f"Model file not found at {self.model_path}") + return None + + TRT_LOGGER = trt.Logger(trt.Logger.WARNING) + with open(self.engine_path, "rb") as f, trt.Runtime(TRT_LOGGER) as runtime: + engine = runtime.deserialize_cuda_engine(f.read()) + refitter = trt.Refitter(engine, TRT_LOGGER) + parser_refitter = trt.OnnxParserRefitter(refitter, TRT_LOGGER) + assert parser_refitter.refit_from_file(self.model_path) + assert refitter.refit_cuda_engine() + return engine + +# fixed allocation: does not account for multiple bindings/batch sizes (single input -> output tensor) +def allocate_buffers(self): + engine = self.engine + # Create a CUDA stream for async execution + self.stream = cuda.Stream() + + self.input_shape = engine.get_binding_shape(0) + self.output_shape = engine.get_binding_shape(1) + + # Allocate device memory for input/output + self.d_input = cuda.mem_alloc(trt.volume(self.input_shape) * np.dtype(np.float32).itemsize) + self.d_output = cuda.mem_alloc(trt.volume(self.output_shape) * np.dtype(np.float32).itemsize) + + # Allocate host pinned memory for input/output (pinned memory for input/output buffers) + self.h_input = cuda.pagelocked_empty(trt.volume(self.input_shape), dtype=np.float32) + self.h_output = cuda.pagelocked_empty(trt.volume(self.output_shape), dtype=np.float32) + + # Example image (allocate on GPU) + self.cv_image = np.random.rand(480, 640, 3).astype(np.uint8) + self.cv_cuda_image = cv2_cuda_GpuMat(self.cv_image.shape[0], self.cv_image.shape[1], cv2.CV_8UC3) + + # Upload image to GPU (device memory) + self.cv_cuda_image.upload(self.cv_image) + + # # Create CUDA IPC handle for output tensor and image + # self.output_ipc_handle = cuda.mem_get_ipc_handle(self.d_output) + # self.image_ipc_handle = cuda.mem_get_ipc_handle(self.cv_cuda_image.cudaPtr()) \ No newline at end of file diff --git a/python_wip/Object Count Tracker.py b/python_wip/tracker.py similarity index 100% rename from python_wip/Object Count Tracker.py rename to python_wip/tracker.py diff --git a/python_wip/velocity.py b/python_wip/velocity.py deleted file mode 100644 index 43bcd35..0000000 --- a/python_wip/velocity.py +++ /dev/null @@ -1,47 +0,0 @@ -import cv2 -import numpy as np -import os - -os.chdir("23-I-12_SysArch/Experiments/Calibration Utilities") - -class VelocityTracker: - def __init__(self, initial_image, point_to_track=np.array([[300, 300]], dtype=np.float32)): - self.point_to_track = point_to_track - self.prev_image = cv2.cvtColor(initial_image, cv2.COLOR_BGR2GRAY) - self.prev_timestamp = None - - def calculate_velocity(self, current_image, current_timestamp): - current_image = cv2.cvtColor(current_image, cv2.COLOR_BGR2GRAY) - new_points, status, error = cv2.calcOpticalFlowPyrLK(self.prev_image, current_image, self.point_to_track, None) - distance = np.sqrt(np.sum((self.point_to_track - new_points) ** 2)) - - if self.prev_timestamp is not None: - time_diff = current_timestamp - self.prev_timestamp - velocity = distance / time_diff if time_diff != 0 else 0 - else: - velocity = 0 - - cv2.circle(current_image, (new_points[0][0], new_points[0][1]), 5, (0, 255, 0), -1) - cv2.putText(current_image, f'Velocity: {velocity}', (new_points[0][0], new_points[0][1] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - cv2.imshow('Image', current_image) - cv2.waitKey(0) - - # Update the previous image, point to track and timestamp - self.prev_image = current_image.copy() - self.point_to_track = new_points - self.prev_timestamp = current_timestamp - - return velocity - - cv2.destroyAllWindows() - -# from velocity import VelocityTracker - -# initial_image = cv2.imread('initial_image.jpg') -# tracker = VelocityTracker(initial_image) - -# # For each new image and timestamp -# new_image = cv2.imread('new_image.jpg') -# new_timestamp = 1234567890 # replace with your timestamp -# offset = tracker.calculate_velocity(new_image, new_timestamp) -offset = velocity * self.latency \ No newline at end of file diff --git a/python_wip/velocityexample.py b/python_wip/velocityexample.py deleted file mode 100644 index 8112830..0000000 --- a/python_wip/velocityexample.py +++ /dev/null @@ -1,160 +0,0 @@ -from ultralytics import YOLO -import supervision as sv -import numpy as np -import os -import cv2 -import time - -# shift should be velocity * latency processing - -os.chdir('/Users/kori0909/Downloads') -model = YOLO("yolov8n.pt") -tracker = sv.ByteTrack() -vidObj = cv2.VideoCapture('topdown.mp4') - -frame_id = 0 -centroids = {} -velocities = {} -boxes = {} -frames = {} -times = {} - -def compute_velocity(tracker_id, centroid_x, centroid_y, scale_factor): - - prev_x, prev_y = centroids.get(tracker_id, (centroid_x, centroid_y)) - frame_count = frames.get(tracker_id, 1) - - - dx = (centroid_x - prev_x) * scale_factor - dy = (centroid_y - prev_y) * scale_factor - - - velocity = np.sqrt(dx**2 + dy**2) / frame_count * 30 - velocities[tracker_id] = round(velocity, 1) - -def annotate_frame(frame, x1, y1, x2, y2, tracker_id): - # Round bounding box coordinates - x1, x2, y1, y2 = int(x1), int(x2), int(y1), int(y2) - - # Draw bounding box and velocity label - frame = cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2) - frame = cv2.putText(frame, f"{velocities[tracker_id]}", (x2+7, y2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 1) - return frame - -def collision_frame(frame, tracker_id, other_tracker_id): - # Threshold for collision detection (Major) - if velocities[tracker_id] > 10 or velocities[other_tracker_id] > 10: - box1 = boxes[tracker_id] - box2 = boxes[other_tracker_id] - - x1_1, y1_1, x2_1, y2_1 = map(int, box1) - x1_2, y1_2, x2_2, y2_2 = map(int, box2) - - label_x1 = int((x1_1 + x2_1) / 2) - label_y1 = int((y1_1 + y2_1) / 2) - - label_x2 = int((x1_2 + x2_2) / 2) - label_y2 = int((y1_2 + y2_2) / 2) - - frame = cv2.rectangle(frame, (x1_1, y1_1), (x2_1, y2_1), (0, 0, 255), 2) - frame = cv2.rectangle(frame, (x1_2, y1_2), (x2_2, y2_2), (0, 0, 255), 2) - - frame = cv2.putText(frame, "Major Collision Detected!", (label_x1, label_y1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - frame = cv2.putText(frame, "Major Collision Detected!", (label_x2, label_y2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - return frame - -# Function to detect collision between two bounding boxes -def detect_collision(box1, box2): - x1_1, y1_1, x2_1, y2_1 = box1 - x1_2, y1_2, x2_2, y2_2 = box2 - - # Detect if either corners of the boxes are overlapping - if x2_1 < x1_2 or x2_2 < x1_1: - return False - - if y2_1 < y1_2 or y2_2 < y1_1: - return False - - return True - -cap = cv2.VideoCapture(0) -annotated_frame = None -collision_times = {} - -while cap.isOpened(): - # Read a frame from the video - success, frame = cap.read() - - if success: - # Increment frame ID - frame_id += 1 - - # Copy the frame for later use - uncropped = frame.copy() - - # Crop the frame to only include the region of interest - frame = frame[220:530, :] - - # Perform inference on the frame - results = model(frame)[0] - - # Format detections object - detections = sv.Detections.from_ultralytics(results) - - # Update the tracker with the detections - detections = tracker.update_with_detections(detections) - - for tracker_id, box, class_id in zip(detections.tracker_id, detections.xyxy, detections.class_id): - # Only process the detection if the class ID is 0 (person) - if class_id == 0: - x1, y1, x2, y2 = box - y1, y2 = y1 + 220, y2 + 220 - boxes[tracker_id] = (x1, y1, x2, y2) - - # Calculate centroid of the bounding box - centroid_x = (x1 + x2) / 2 - centroid_y = (y1 + y2) / 2 - ret, frame = cap.read() - - if ret: - # Get frame dimensions - frame_height, frame_width = frame.shape[:2] - # Compute velocity using previous centroids - compute_velocity(tracker_id, centroid_x, centroid_y, scale_factor=1) - # Update centroid history to newest - centroids[tracker_id] = (centroid_x, centroid_y) - # Annotate frame - annotated_frame = annotate_frame(uncropped, x1, y1, x2, y2, tracker_id) - frames[tracker_id] = frame_id - - # Check for collisions - for other_tracker_id, other_box in boxes.items(): - if other_tracker_id != tracker_id: - if detect_collision(boxes[tracker_id], boxes[other_tracker_id]): - collision_times[(tracker_id, other_tracker_id)] = time.time() - annotated_frame = collision_frame(uncropped, tracker_id, other_tracker_id) - elif (tracker_id, other_tracker_id) in collision_times and time.time() - collision_times[(tracker_id, other_tracker_id)] <= 5: - annotated_frame = collision_frame(uncropped, tracker_id, other_tracker_id) - else: - annotated_frame = annotate_frame(uncropped, x1, y1, x2, y2, tracker_id) - - # Remove entries from collision_times if they have been there for more than 2 seconds - current_time = time.time() - collision_times = {k: v for k, v in collision_times.items() if current_time - v <= 2} - - # Display the annotated frame - if annotated_frame is not None: - cv2.imshow("YOLOv8 Inference", annotated_frame) - - # Break the loop if 'q' is pressed - if cv2.waitKey(1) & 0xFF == ord("q"): - break - else: - # Break if end of video is reached - break - -# Release the video capture object -cap.release() -# Close the display window -cv2.destroyAllWindows() \ No newline at end of file diff --git a/workspace_python/ros2_ws/src/python_workspace/README.md b/workspace_python/ros2_ws/src/python_workspace/README.md new file mode 100644 index 0000000..9527dcf --- /dev/null +++ b/workspace_python/ros2_ws/src/python_workspace/README.md @@ -0,0 +1,2 @@ +Running the camera node: +ros2 run python_workspace camera_node --ros-args -p source_type:=static_image -p static_image_path:='/home/user/Desktop/ROS/Models/Maize Model/sample_maize_images' -p loop:=-1 -p frame_rate:=10 -p model_dimensions:=[640,640] \ No newline at end of file diff --git a/workspace_python/ros2_ws/src/python_workspace/launch/benchmarking.py b/workspace_python/ros2_ws/src/python_workspace/launch/benchmarking.py new file mode 100644 index 0000000..e69de29 diff --git a/workspace_python/ros2_ws/src/python_workspace/launch/composable.py b/workspace_python/ros2_ws/src/python_workspace/launch/composable.py new file mode 100644 index 0000000..ac54874 --- /dev/null +++ b/workspace_python/ros2_ws/src/python_workspace/launch/composable.py @@ -0,0 +1,218 @@ +## for composable: +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + container = ComposableNodeContainer( + name='my_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='mypackage.ComposablePublisherNode', + name='composable_publisher_node' + ), + ], + output='screen', + ) + + lifecycle_manager = LifecycleNode( + package='my_package', + executable='lifecycle_node', + name='lifecycle_manager_node', + namespace='', + output='screen', + ) + + return LaunchDescription([ + container, + lifecycle_manager + ]) + +# new basic: +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='my_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='MyNode', + name='my_node' + ) + ], + output='screen', + ), + ]) + +# This launch file will load MyNode into the container at runtime. + +## basic pt 2 +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='my_composable_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', # Multi-threaded container + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='my_package.IndoorNavNode', + name='indoor_nav' + ), + ComposableNode( + package='my_package', + plugin='my_package.LiDARNode', + name='lidar_node' + ), + ], + output='screen', + ), + ]) + +## cuda composable: +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch import LaunchDescription + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='gpu_composable_container', + namespace='', + package='rclpy_components', + executable='component_container_mt', # Multi-threaded container for composable nodes + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='my_package.image_preprocessing_node', + name='image_preprocessing_node' + ), + ComposableNode( + package='my_package', + plugin='my_package.inference_node', + name='inference_node', + parameters=[ + {"preprocess_node": "image_preprocessing_node"} + ] + ), + ], + output='screen', + ), + ]) + + + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='my_python_container', + namespace='', + package='rclpy_components', + executable='component_container_mt', # Multi-threaded container + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='my_python_node.MyPythonNode', + name='my_python_node' + ) + ], + output='screen', + ), + ]) + +### ll + + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='camera_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='your_package_name', + plugin='your_package_name.CameraNode', + name='camera_node', + parameters=[{ + 'source_type': 'zed', + 'static_image_path': '.../assets/', + 'video_path': '.../assets/video.mp4', + 'loop': 0, + 'frame_rate': 30, + 'model_type': 'maize', + }], + ), + ], + output='screen', + ), + ]) + +# cuda concurrent +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch import LaunchDescription +import threading + +sync_event = threading.Event() + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name='gpu_composable_container', + namespace='', + package='rclpy_components', + executable='component_container_mt', # Multi-threaded container for composable nodes + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='my_package.ImagePreprocessingNode', + name='image_preprocessing_node', + parameters=[ + {"sync_event": sync_event} + ] + ), + ComposableNode( + package='my_package', + plugin='my_package.InferenceNode', + name='inference_node', + parameters=[ + {"preprocess_node": "image_preprocessing_node", "sync_event": sync_event} + ] + ), + ], + output='screen', + ), + ]) + +# CUDA Streams for Asynchronous Execution: Both the preprocessing and inference nodes now use CUDA streams to allow for non-blocking GPU operations. + +# Image Preprocessing Node: Executes memcpy_htod_async to copy the image to GPU memory without blocking the CPU. The preprocessing stream is synchronized with stream.synchronize() to ensure the operation completes. +# Inference Node: Executes inference in its own stream using execute_async_v2. The stream is synchronized with stream.synchronize() to ensure that inference completes before retrieving the results. + +# Concurrency: By using a multi-threaded composable node container (component_container_mt), both nodes can process different images in parallel. This means the preprocessing node can start working on the next image while the inference node is still running on the previous one. \ No newline at end of file diff --git a/workspace_python/ros2_ws/src/python_workspace/launch/debugging.py b/workspace_python/ros2_ws/src/python_workspace/launch/debugging.py new file mode 100644 index 0000000..e69de29 diff --git a/workspace_python/ros2_ws/src/python_workspace/launch/launch.py b/workspace_python/ros2_ws/src/python_workspace/launch/launch.py index b0d80f9..c631358 100644 --- a/workspace_python/ros2_ws/src/python_workspace/launch/launch.py +++ b/workspace_python/ros2_ws/src/python_workspace/launch/launch.py @@ -7,6 +7,9 @@ from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition +# update parameters +# specify side to camera_node and jetson_node for left/right subscriber/topic +# should be two composition containers for left/right def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( @@ -122,130 +125,4 @@ def generate_launch_description(): {'publish_rate': LaunchConfiguration('publish_rate')}, ] ), - ]) - -# ## for composable: -# # example_launch.py -# from launch import LaunchDescription -# from launch_ros.actions import LifecycleNode -# from launch_ros.actions import ComposableNodeContainer -# from launch_ros.descriptions import ComposableNode - -# def generate_launch_description(): -# container = ComposableNodeContainer( -# name='my_container', -# namespace='', -# package='rclcpp_components', -# executable='component_container_mt', -# composable_node_descriptions=[ -# ComposableNode( -# package='my_package', -# plugin='mypackage.ComposablePublisherNode', -# name='composable_publisher_node' -# ), -# ], -# output='screen', -# ) - -# lifecycle_manager = LifecycleNode( -# package='my_package', -# executable='lifecycle_node', -# name='lifecycle_manager_node', -# namespace='', -# output='screen', -# ) - -# return LaunchDescription([ -# container, -# lifecycle_manager -# ]) - -# ## parameter handling: -# from launch import LaunchDescription -# from launch_ros.actions import LifecycleNode -# from launch_ros.actions import ComposableNodeContainer -# from launch_ros.descriptions import ComposableNode -# from launch.actions import LogInfo, EmitEvent -# from launch.events.lifecycle import ChangeState -# from lifecycle_msgs.msg import Transition - -# def generate_launch_description(): -# # Define parameters to pass to the composable node -# composable_params = { -# 'message': 'This is a custom message from launch!', -# 'publish_frequency': 1.0 -# } - -# container = ComposableNodeContainer( -# name='my_container', -# namespace='', -# package='rclcpp_components', -# executable='component_container_mt', -# composable_node_descriptions=[ -# ComposableNode( -# package='my_package', -# plugin='mypackage.ComposablePublisherNode', -# name='composable_publisher_node', -# parameters=[composable_params] -# ), -# ComposableNode( -# package='my_package', -# plugin='mypackage.ComposablePublisherNode', -# name='additional_publisher_node', -# parameters=[{'message': 'This is the second node!', 'publish_frequency': 2.5}] -# ) -# ], -# output='screen', -# ) - -# lifecycle_manager = LifecycleNode( -# package='my_package', -# executable='lifecycle_node', -# name='lifecycle_manager_node', -# namespace='', -# output='screen', -# parameters=[{'lifecycle_action': 'configure'}] -# ) - -# # Manually trigger lifecycle transitions after nodes are up -# configure_event = EmitEvent( -# event=ChangeState( -# lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), -# transition_id=Transition.TRANSITION_CONFIGURE -# ) -# ) - -# activate_event = EmitEvent( -# event=ChangeState( -# lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), -# transition_id=Transition.TRANSITION_ACTIVATE -# ) -# ) - -# deactivate_event = EmitEvent( -# event=ChangeState( -# lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), -# transition_id=Transition.TRANSITION_DEACTIVATE -# ) -# ) - -# shutdown_event = EmitEvent( -# event=ChangeState( -# lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), -# transition_id=Transition.TRANSITION_SHUTDOWN -# ) -# ) - -# return LaunchDescription([ -# LogInfo(msg="Starting the container and lifecycle manager..."), -# container, -# lifecycle_manager, -# LogInfo(msg="Configuring the lifecycle node..."), -# configure_event, -# LogInfo(msg="Activating the lifecycle node..."), -# activate_event, -# LogInfo(msg="Deactivating the lifecycle node..."), -# deactivate_event, -# LogInfo(msg="Shutting down the lifecycle node..."), -# shutdown_event, -# ]) \ No newline at end of file + ]) \ No newline at end of file diff --git a/python_wip/composable_lifecycle_launch.py b/workspace_python/ros2_ws/src/python_workspace/launch/lifecycle.py similarity index 67% rename from python_wip/composable_lifecycle_launch.py rename to workspace_python/ros2_ws/src/python_workspace/launch/lifecycle.py index 2e0393e..1b928c9 100644 --- a/python_wip/composable_lifecycle_launch.py +++ b/workspace_python/ros2_ws/src/python_workspace/launch/lifecycle.py @@ -1,3 +1,37 @@ +import rclpy +from rclpy.node import Node +from rclpy.lifecycle import LifecycleNode +from rclpy.lifecycle import State + +class CameraNode(LifecycleNode): + def __init__(self): + super().__init__('camera_node') + + def on_configure(self, state: State): + self.get_logger().info('Configuring the camera...') + # Initialize camera + return self.configure_success() + + def on_activate(self, state: State): + self.get_logger().info('Activating the camera...') + # Start the camera stream + return self.activate_success() + + def on_deactivate(self, state: State): + self.get_logger().info('Deactivating the camera...') + # Stop the camera stream + return self.deactivate_success() + + def on_cleanup(self, state: State): + self.get_logger().info('Cleaning up resources...') + # Clean up resources + return self.cleanup_success() + + def on_shutdown(self, state: State): + self.get_logger().info('Shutting down the camera...') + # Shut down resources + return self.shutdown_success() + from launch import LaunchDescription from launch_ros.actions import LifecycleNode from launch.actions import EmitEvent diff --git a/workspace_python/ros2_ws/src/python_workspace/launch/parameters.py b/workspace_python/ros2_ws/src/python_workspace/launch/parameters.py new file mode 100644 index 0000000..5dbf21a --- /dev/null +++ b/workspace_python/ros2_ws/src/python_workspace/launch/parameters.py @@ -0,0 +1,89 @@ +## parameter handling: +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import LogInfo, EmitEvent +from launch.events.lifecycle import ChangeState +from lifecycle_msgs.msg import Transition + +def generate_launch_description(): + # Define parameters to pass to the composable node + composable_params = { + 'message': 'This is a custom message from launch!', + 'publish_frequency': 1.0 + } + + container = ComposableNodeContainer( + name='my_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='my_package', + plugin='mypackage.ComposablePublisherNode', + name='composable_publisher_node', + parameters=[composable_params] + ), + ComposableNode( + package='my_package', + plugin='mypackage.ComposablePublisherNode', + name='additional_publisher_node', + parameters=[{'message': 'This is the second node!', 'publish_frequency': 2.5}] + ) + ], + output='screen', + ) + + lifecycle_manager = LifecycleNode( + package='my_package', + executable='lifecycle_node', + name='lifecycle_manager_node', + namespace='', + output='screen', + parameters=[{'lifecycle_action': 'configure'}] + ) + + # Manually trigger lifecycle transitions after nodes are up + configure_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), + transition_id=Transition.TRANSITION_CONFIGURE + ) + ) + + activate_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), + transition_id=Transition.TRANSITION_ACTIVATE + ) + ) + + deactivate_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), + transition_id=Transition.TRANSITION_DEACTIVATE + ) + ) + + shutdown_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(lifecycle_manager), + transition_id=Transition.TRANSITION_SHUTDOWN + ) + ) + + return LaunchDescription([ + LogInfo(msg="Starting the container and lifecycle manager..."), + container, + lifecycle_manager, + LogInfo(msg="Configuring the lifecycle node..."), + configure_event, + LogInfo(msg="Activating the lifecycle node..."), + activate_event, + LogInfo(msg="Deactivating the lifecycle node..."), + deactivate_event, + LogInfo(msg="Shutting down the lifecycle node..."), + shutdown_event, + ]) \ No newline at end of file diff --git a/workspace_python/ros2_ws/src/python_workspace/python_workspace/camera_node.py b/workspace_python/ros2_ws/src/python_workspace/python_workspace/camera_node.py index d02be31..2af0934 100644 --- a/workspace_python/ros2_ws/src/python_workspace/python_workspace/camera_node.py +++ b/workspace_python/ros2_ws/src/python_workspace/python_workspace/camera_node.py @@ -1,6 +1,8 @@ -import time, sys, os +import time, os import cv2 import pyzed.sl as sl +import pycuda.driver as cuda +import cupy as cp import numpy as np import rclpy @@ -8,20 +10,28 @@ from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from sensor_msgs.msg import Image -from std_msgs.msg import Header +from std_msgs.msg import Header, String from cv_bridge import CvBridge +cuda.init() +device = cuda.Device(0) +cuda_driver_context = device.make_context() + class CameraNode(Node): def __init__(self): super().__init__('camera_node') + # change defaults here self.declare_parameter('source_type', 'zed') # static_image, video, zed - self.declare_parameter('static_image_path', '.../assets/') - self.declare_parameter('video_path', '.../assets/video.mp4') + self.declare_parameter('static_image_path', '/home/usr/Desktop/ROS/assets/IMG_1822_14.JPG') + self.declare_parameter('video_path', '/home/usr/Desktop/ROS/assets/video.mp4') self.declare_parameter('loop', 0) # 0 = don't loop, >0 = # of loops, -1 = loop forever self.declare_parameter('frame_rate', 30) # Desired frame rate for publishing self.declare_parameter('model_dimensions', (448, 1024)) - # self.declare_parameter('camera_serial_number', 1101010101) + self.declare_parameter('camera_side', 'left') # left, right + self.declare_parameter('shift_constant', 1) + self.declare_parameter('roi_dimensions', [0, 0, 100, 100]) + # propagate fp16 option (.fp16 datatype for cupy) self.source_type = self.get_parameter('source_type').get_parameter_value().string_value self.static_image_path = self.get_parameter('static_image_path').get_parameter_value().string_value @@ -29,11 +39,14 @@ def __init__(self): self.loop = self.get_parameter('loop').get_parameter_value().integer_value self.frame_rate = self.get_parameter('frame_rate').get_parameter_value().integer_value self.dimensions = tuple(self.get_parameter('model_dimensions').get_parameter_value().integer_array_value) - # self.serial_number = self.get_parameter('camera_serial_number').get_parameter_value().integer_value + self.camera_side = self.get_parameter('camera_side').get_parameter_value().string_value + self.shift_constant = self.get_parameter('shift_constant').get_parameter_value().integer_value + self.roi_dimensions = self.get_parameter('roi_dimensions').get_parameter_value().integer_array_value - self.publisher = self.create_publisher(Image, 'image_data', 10) + self.pointer_publisher = self.create_publisher(String, 'preprocessing_done', 10) + self.image_publisher = self.create_publisher(Image, 'image_data', 10) self.bridge = CvBridge() - self.index = 0 + self.velocity = 0 if self.source_type == 'static_image': self.publish_static_image() @@ -43,23 +56,36 @@ def __init__(self): self.publish_zed_frames() else: self.get_logger().error(f"Invalid source_type: {self.source_type}") + return def publish_static_image(self): if not os.path.exists(self.static_image_path): self.get_logger().error(f"Static image not found at {self.static_image_path}") + # raise FileNotFoundError(f"Static image not found at {self.static_image_path}") + return + + images = [] + if os.path.isdir(self.static_image_path): + for filename in os.listdir(self.static_image_path): + if filename.endswith('.JPG') or filename.endswith('.png'): + images.append(os.path.join(self.static_image_path, filename)) + elif os.path.isfile(self.static_image_path): + images.append(self.static_image_path) + + if len(images) == 0: + self.get_logger().error(f"No images found at {self.static_image_path}") return loops = 0 while rclpy.ok() and (self.loop == -1 or loops < self.loop): - for filename in os.listdir(self.static_image_path): - if filename.endswith('.JPG') or filename.endswith('.png'): - # print("found") !! log properly for vid too - image = cv2.imread(os.path.join(self.static_image_path, filename), cv2.IMREAD_COLOR) - if image is not None: - self.index += 1 - image = cv2.resize(image, (self.dimensions)) - self.publish_image(image) - time.sleep(1.0 / self.frame_rate) # delay to control framerate + for filename in images: + image = cv2.imread(filename, cv2.IMREAD_COLOR) + if image is not None: + self.preprocess_image(image) + time.sleep(1.0 / self.frame_rate) + else: + self.get_logger().error(f"Failed to read image: {filename}") + raise FileNotFoundError(f"Failed to read image: {filename}") if self.loop > 0: loops += 1 @@ -71,7 +97,7 @@ def publish_video_frames(self): cap = cv2.VideoCapture(self.video_path) if not cap.isOpened(): self.get_logger().error(f"Failed to open video: {self.video_path}") - return + return loops = 0 while rclpy.ok() and (self.loop == -1 or loops < self.loop): @@ -79,16 +105,14 @@ def publish_video_frames(self): ret, frame = cap.read() if not ret: break - self.index += 1 - self.publish_image(frame) - time.sleep(1.0 / self.frame_rate) # delay to control framerate + self.preprocess_image(frame) + time.sleep(1.0 / self.frame_rate) if self.loop > 0: loops += 1 if self.loop != -1: cap.set(cv2.CAP_PROP_POS_FRAMES, 0) # restart video - cap.release() def publish_zed_frames(self): @@ -96,8 +120,13 @@ def publish_zed_frames(self): init = sl.InitParameters() init.camera_resolution = sl.RESOLUTION.HD1080 init.camera_fps = 30 # do we need publisher delay if this param is here? - # init.set_from_serial_number(self.serial_number) # or give side and manually convert - + + if self.side == 'left': + init.camera_linux_id = 0 + elif self.side == 'right': + init.camera_linux_id = 1 + self.shift_constant *= -1 + if not zed.is_opened(): print("Opening ZED Camera ") status = zed.open(init) @@ -107,7 +136,6 @@ def publish_zed_frames(self): runtime = sl.RuntimeParameters() image_zed = sl.Mat() - sensors_data = sl.SensorsData() previous_velocity = np.array([0.0, 0.0, 0.0]) previous_time = time.time() @@ -115,9 +143,10 @@ def publish_zed_frames(self): while rclpy.ok(): err = zed.grab(runtime) if err == sl.ERROR_CODE.SUCCESS: - self.index += 1 - tic = time.perf_counter_ns() - zed.retrieve_image(image_zed, sl.VIEW.LEFT_UNRECTIFIED) # modify based on left/right + if self.side == 'left': + zed.retrieve_image(image_zed, sl.VIEW.LEFT_UNRECTIFIED) + else: + zed.retrieve_image(image_zed, sl.VIEW.RIGHT_UNRECTIFIED) zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.IMAGE) accel_data = sensors_data.get_imu_data().get_linear_acceleration() @@ -126,51 +155,70 @@ def publish_zed_frames(self): delta_time = current_time - previous_time acceleration = np.array([accel_data[0], accel_data[1], accel_data[2]]) velocity = previous_velocity + acceleration * delta_time + self.velocity = velocity previous_velocity = velocity previous_time = current_time - # Take and transform image using CUDA operations - cv_image = image_zed.get_data() - gpu_image = cv2.cuda_GpuMat() - gpu_image.upload(cv_image) - gpu_image = cv2.cuda.cvtColor(gpu_image, cv2.COLOR_RGBA2RGB) - # crop goes here... - gpu_image = cv2.cuda.resize(gpu_image, self.dimensions) + cv_image = image_zed.get_data() + self.preprocess_image(cv_image) + time.sleep(1.0 / self.frame_rate) - # Download the processed image from GPU to CPU memory - rgb_image = gpu_image.download() - toc = time.perf_counter_ns() - self.get_logger().info(f"Preprocessing: {(toc - tic)/1e6} ms") - self.publish_image(rgb_image) - time.sleep(1.0 / self.frame_rate) # delay to control framerate else: self.get_logger().error("Failed to grab ZED camera frame: {str(err)}") zed.close() - print("ZED Camera closed") + print("ZED Camera closed") + + def preprocess_image(self, image): + tic = time.perf_counter_ns() + + roi_x, roi_y, roi_w, roi_h = self.roi_dimensions + shifted_x = roi_x + abs(self.velocity[0]) * self.shift_constant + + gpu_image = cv2.cuda_GpuMat() + gpu_image.upload(image) + + gpu_image = cv2.cuda.cvtColor(gpu_image, cv2.COLOR_RGBA2RGB) # remove alpha channel + gpu_image = gpu_image[roi_y:(roi_y+roi_h), shifted_x:(shifted_x+roi_w)] # crop the image to ROI + gpu_image = cv2.cuda.resize(gpu_image, self.dimensions) # resize to model dimensions + + input_data = cp.asarray(gpu_image.download(), dtype=cp.float32) # Now the image is on GPU memory as CuPy array + input_data /= 255.0 # normalize to [0, 1] + input_data = cp.transpose(input_data, (2, 0, 1)) # Transpose from HWC (height, width, channels) to CHW (channels, height, width) + d_input_ptr = input_data.data.ptr # Get device pointer of the CuPy array + ipc_handle = cuda.mem_get_ipc_handle(d_input_ptr) # Create the IPC handle + + toc = time.perf_counter_ns() + self.get_logger().info(f"Preprocessing: {(toc-tic)/1e6} ms") + + # Publish the IPC handle as a string (sending the handle reference as a string) + ipc_handle_msg = String() + ipc_handle_msg.data = str(ipc_handle.handle) + self.pointer_publisher.publish(ipc_handle_msg) + def publish_image(self, image): header = Header() header.stamp = self.get_clock().now().to_msg() - header.frame_id = str(self.index) - # try packing velcoity information into header + # header.frame_id = str(self.index) # encode the transformation data into header image_msg = self.bridge.cv2_to_imgmsg(image, encoding="rgb8") - image_msg.header = header image_msg.is_bigendian = 0 image_msg.step = image_msg.width * 3 - self.publisher.publish(image_msg) def main(args=None): rclpy.init(args=args) camera_node = CameraNode() - executor = MultiThreadedExecutor() + executor = MultiThreadedExecutor(num_threads=1) executor.add_node(camera_node) - executor.spin() - camera_node.display_metrics() - camera_node.destroy_node() - rclpy.shutdown() + + try: + executor.spin() + finally: + executor.shutdown() + camera_node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': main() \ No newline at end of file diff --git a/workspace_python/ros2_ws/src/python_workspace/python_workspace/extermination_node.py b/workspace_python/ros2_ws/src/python_workspace/python_workspace/extermination_node.py index a19420c..d822421 100644 --- a/workspace_python/ros2_ws/src/python_workspace/python_workspace/extermination_node.py +++ b/workspace_python/ros2_ws/src/python_workspace/python_workspace/extermination_node.py @@ -3,6 +3,7 @@ import numpy as np # from tracker import * # add object counter +# log end to end latency by checking time.time_now() and unpacking time from header of initial image after the extermination output is decided import rclpy from rclpy.node import Node @@ -11,6 +12,9 @@ from std_msgs.msg import Header, String, Integer from cv_bridge import CvBridge, CvBridgeError +import pycuda.driver as cuda + + class ExterminationNode(Node): def __init__(self): super().__init__('extermination_node') @@ -30,6 +34,11 @@ def __init__(self): self.roi_list = self.get_parameter('roi_list').get_parameter_value().integer_array_value self.publish_rate = self.get_parameter('publish_rate').get_parameter_value().integer_value + self.subscription_pointer = self.create_subscription(MemoryHandle, 'inference_done', self.postprocess_callback, 10) + # Ensure same CUDA context or initialize a new one if needed + self.cuda_driver_context = cuda.Device(0).make_context() + + self.subscription_img = self.create_subscription(Image, 'image', self.image_callback, 10) self.subscription_bbox = self.create_subscription(String, 'bounding_boxes', self.bbox_callback, 10) self.timer = self.create_timer(0.1, self.timer_callback) @@ -50,6 +59,63 @@ def __init__(self): self.window2 = "Right Camera" cv2.namedWindow(self.window1) cv2.namedWindow(self.window2) + + def image_callback(self, msg): + self.arrival_time = Time.from_msg(msg.header.stamp) + image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8") + self.image = image + latency = self.get_clock().now() - Time.from_msg(msg.header.stamp) + self.get_logger().info(f"Latency: {latency.nanoseconds / 1e6} ms") + self.preprocess(image) + + def postprocess_callback(self, msg): + ipc_handle_str = msg.ipc_handle + ipc_handle = cuda.IPCMemoryHandle(ipc_handle_str) + + # Map the shared GPU memory using IPC handle + d_output = cuda.ipc_open_mem_handle(ipc_handle, cuda.mem_alloc(self.h_output.nbytes)) + + # Copy data from GPU memory to host + cuda.memcpy_dtoh(self.h_output, d_output) + + # Synchronize and postprocess + output = np.copy(self.h_output) + self.stream.synchronize() + + self.get_logger().info(f"Postprocessed output: {output}") + + # Clean up the IPC memory + cuda.ipc_close_mem_handle(d_output) + + # Get the IPC handles for tensor and image + tensor_ipc_handle_str = msg.tensor_ipc_handle + image_ipc_handle_str = msg.image_ipc_handle + + # Open IPC memory handles for tensor and image + tensor_ipc_handle = cuda.IPCMemoryHandle(tensor_ipc_handle_str) + image_ipc_handle = cuda.IPCMemoryHandle(image_ipc_handle_str) + + d_output = cuda.ipc_open_mem_handle(tensor_ipc_handle, self.h_output.nbytes) + d_image = cuda.ipc_open_mem_handle(image_ipc_handle, self.cv_image.nbytes) + + # Wrap the image GPU pointer into a GpuMat object for OpenCV CUDA operations + cv_cuda_image = cv2_cuda_GpuMat(self.cv_image.shape[0], self.cv_image.shape[1], cv2.CV_8UC3) + cv_cuda_image.upload(d_image) + + # Perform OpenCV CUDA operations on the image (e.g., GaussianBlur) + blurred_image = cv2_cuda_image.gaussianBlur((5, 5), 0) + + # Retrieve inference result and postprocess + cuda.memcpy_dtoh(self.h_output, d_output) + self.stream.synchronize() + + output = np.copy(self.h_output) + self.get_logger().info(f"Postprocessed tensor: {output}") + + # Clean up IPC memory handles + cuda.ipc_close_mem_handle(d_output) + cuda.ipc_close_mem_handle(d_image) + def image_callback(self, msg): if msg.header.side == "left": @@ -85,6 +151,26 @@ def timer_callback(self, side): Rmsg.data = self.right.on self.left_array_publisher.publish(Lmsg) self.right_array_publisher.publish(Rmsg) + + # def nms(self, boxes, threshold=0.5): + # x1 = boxes[:, 0] + # y1 = boxes[:, 1] + # x2 = boxes[:, 2] + # y2 = boxes[:, 3] + # scores = boxes[:, 4] + + # indices = np.argsort(scores)[::-1] + # keep = [] + + # while len(indices) > 0: + # i = indices[0] + # keep.append(i) + + # if len(indices) == 1: + # break + + # remaining_boxes = boxes[indices[1:]] + # iou_values = np.array([self.iou(boxes[i], remaining_box) for remaining_box in remaining_boxes]) def process_data(self, side): if side == "left": @@ -205,4 +291,152 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() + +import cupy as cp + +def iou(box1, box2): + """Compute Intersection over Union (IoU) between two bounding boxes. + + Args: + box1: (x1, y1, x2, y2) format for box 1. + box2: (x1, y1, x2, y2) format for box 2. + + Returns: + IoU value between box1 and box2. + """ + # Determine the coordinates of the intersection rectangle + x1 = cp.maximum(box1[0], box2[0]) + y1 = cp.maximum(box1[1], box2[1]) + x2 = cp.minimum(box1[2], box2[2]) + y2 = cp.minimum(box1[3], box2[3]) + + # Compute the area of the intersection + inter_area = cp.maximum(0, x2 - x1 + 1) * cp.maximum(0, y2 - y1 + 1) + + # Compute the area of both bounding boxes + box1_area = (box1[2] - box1[0] + 1) * (box1[3] - box1[1] + 1) + box2_area = (box2[2] - box2[0] + 1) * (box2[3] - box2[1] + 1) + + # Compute the IoU + iou_value = inter_area / (box1_area + box2_area - inter_area) + return iou_value + +def nms(boxes, threshold=0.5): + """Apply Non-Maximum Suppression (NMS) on bounding boxes using CUDA. + + Args: + boxes: Bounding boxes with shape [N, 5], where each box is [x1, y1, x2, y2, score]. + threshold: IoU threshold for suppression. + + Returns: + List of indices of the boxes that are kept after NMS. + """ + # Extract the coordinates and scores from the boxes + x1 = boxes[:, 0] + y1 = boxes[:, 1] + x2 = boxes[:, 2] + y2 = boxes[:, 3] + scores = boxes[:, 4] + + # Sort boxes by scores (descending order) + indices = cp.argsort(scores)[::-1] + + keep = [] + + while len(indices) > 0: + i = indices[0] + keep.append(i) + + if len(indices) == 1: + break + + # Remaining boxes + remaining_boxes = boxes[indices[1:]] + + # Calculate IoU of the current box with the rest + iou_values = cp.array([iou(boxes[i], remaining_box) for remaining_box in remaining_boxes]) + + # Suppress boxes with IoU greater than the threshold + remaining_indices = cp.where(iou_values <= threshold)[0] + + # Update indices to keep processing the remaining boxes + indices = indices[remaining_indices + 1] + + return keep + +# Example usage: + +# Suppose we have the following bounding boxes in (x1, y1, x2, y2, score) format: +bounding_boxes = cp.array([ + [50, 50, 150, 150, 0.9], + [60, 60, 160, 160, 0.85], + [200, 200, 300, 300, 0.75], + [220, 220, 320, 320, 0.7], +]) + +# Apply NMS with IoU threshold of 0.5 +kept_indices = nms(bounding_boxes, threshold=0.5) + +# Display results +print("Kept indices:", kept_indices) +print("Kept boxes:", bounding_boxes[kept_indices]) + +import cv2 +import os + +def draw_bounding_boxes(image_path, bboxes): + # Read the image using OpenCV + img = cv2.imread(image_path) + + if img is None: + print(f"Error loading image: {image_path}") + return + + # Get the dimensions of the image + height, width, _ = img.shape + print(height) + print(width) + + # Draw each bounding box on the image + for bbox in bboxes: + class_id, x_center, y_center, bbox_width, bbox_height = bbox + + # Convert normalized values to absolute pixel values + x_center_pixel = int(x_center * width) + y_center_pixel = int(y_center * height) + bbox_width_pixel = int(bbox_width * width) + bbox_height_pixel = int(bbox_height * height) + + # Calculate the top-left and bottom-right corners of the bounding box + top_left_x = int(x_center_pixel - bbox_width_pixel / 2) + top_left_y = int(y_center_pixel - bbox_height_pixel / 2) + bottom_right_x = int(x_center_pixel + bbox_width_pixel / 2) + bottom_right_y = int(y_center_pixel + bbox_height_pixel / 2) + + # Draw the bounding box (using green color and thickness of 2) + cv2.rectangle(img, (top_left_x, top_left_y), (bottom_right_x, bottom_right_y), (0, 255, 0), 2) + + # Show the image with bounding boxes (press any key to close) + cv2.imshow('Bounding Boxes', img) + cv2.waitKey(10000) + cv2.destroyAllWindows() + +def read_bounding_boxes(txt_file): + bboxes = [] + with open(txt_file, 'r') as file: + for line in file.readlines(): + values = line.strip().split() + class_id = int(values[0]) + x_center = float(values[1]) + y_center = float(values[2]) + bbox_width = float(values[3]) + bbox_height = float(values[4]) + bboxes.append((class_id, x_center, y_center, bbox_width, bbox_height)) + return bboxes + +os.chdir("C:/Users/ishaa/Coding Projects/Applied-AI/ROS/assets/maize") +print(os.getcwd()) +boxes = read_bounding_boxes("IMG_2884_18.txt") +print(boxes) +draw_bounding_boxes("IMG_2884_18.JPG", boxes) \ No newline at end of file diff --git a/workspace_python/ros2_ws/src/python_workspace/python_workspace/jetson_node.py b/workspace_python/ros2_ws/src/python_workspace/python_workspace/jetson_node.py index 2813e52..6bf3744 100644 --- a/workspace_python/ros2_ws/src/python_workspace/python_workspace/jetson_node.py +++ b/workspace_python/ros2_ws/src/python_workspace/python_workspace/jetson_node.py @@ -1,16 +1,14 @@ -import time, os, sys -import numpy as np -import cv2 +import time, os import tensorrt as trt import pycuda.driver as cuda +import cupy as cp +import numpy as np import rclpy from rclpy.time import Time from rclpy.node import Node -from rclpy.executors import MultiThreadedExecutor -from sensor_msgs.msg import Image from std_msgs.msg import Header, String -from cv_bridge import CvBridge +from rclpy.executors import MultiThreadedExecutor cuda.init() device = cuda.Device(0) @@ -18,46 +16,27 @@ class JetsonNode(Node): def __init__(self): - super().__init__('jetson_node') + super().__init__('jetson_node') self.declare_parameter('engine_path', '/home/user/Downloads/model.engine') self.declare_parameter('strip_weights', 'False') - self.declare_parameter('model_path', '/home/user/Downloads/model.onnx') + self.declare_parameter('precision', 'fp32') # fp32, fp16, int8 - self.model_path = self.get_parameter('model_path').get_parameter_value().string_value self.engine_path = self.get_parameter('engine_path').get_parameter_value().string_value self.strip_weights = self.get_parameter('strip_weights').get_parameter_value().bool_value + self.precision = self.get_parameter('precision').get_parameter_value().string_value - self.camera_subscriber = self.create_subscription(Image, 'image_data', self.image_callback, 10) - self.bbox_publisher = self.create_publisher(String, 'bounding_boxes', 10) - self.bridge = CvBridge() - self.arrival_time, self.image = 0, None - - if self.strip_weights: - self.engine = self.load_stripped_engine_and_refit() - else: - self.engine = self.load_normal_engine() - - self.allocate_buffers() - self.exec_context = (self.engine).create_execution_context() - - def load_stripped_engine_and_refit(self): - if not os.path.exists(self.engine_path): - self.get_logger().error(f"Engine file not found at {self.engine_path}") - return None - - if not os.path.exists(self.model_path): - self.get_logger().error(f"Model file not found at {self.model_path}") - return None + self.engine, self.context = self.load_normal_engine() + self.stream = cuda.Stream() + input_shape = (self.engine).get_binding_shape(0) + output_shape = (self.engine).get_binding_shape(1) + self.d_input = cuda.mem_alloc(trt.volume(input_shape) * np.dtype(np.float32).itemsize) # change to fp16, etc. + self.d_output = cuda.mem_alloc(trt.volume(output_shape) * np.dtype(np.float32).itemsize) # Allocate device memory for input/output - TRT_LOGGER = trt.Logger(trt.Logger.WARNING) - with open(self.engine_path, "rb") as f, trt.Runtime(TRT_LOGGER) as runtime: - engine = runtime.deserialize_cuda_engine(f.read()) - refitter = trt.Refitter(engine, TRT_LOGGER) - parser_refitter = trt.OnnxParserRefitter(refitter, TRT_LOGGER) - assert parser_refitter.refit_from_file(self.model_path) - assert refitter.refit_cuda_engine() - return engine + self.pointer_subscriber = self.create_publisher(String, 'preprocessing_done', self.pointer_callback, 10) + self.pointer_publisher = self.create_publisher(String, 'inference_done', 10) + self.arrival_time, self.type = 0, None, None + self.warmup() def load_normal_engine(self): if not os.path.exists(self.engine_path): @@ -67,70 +46,63 @@ def load_normal_engine(self): TRT_LOGGER = trt.Logger(trt.Logger.WARNING) with open(self.engine_path, "rb") as f, trt.Runtime(TRT_LOGGER) as runtime: engine = runtime.deserialize_cuda_engine(f.read()) + context = engine.create_execution_context() self.get_logger().info(f"Successfully loaded engine from {self.engine_path}") - return engine + return engine, context + + # after verifying this works pass in the already allocated memory in class instantiation + def warmup(self): + input_shape = self.context.get_binding_shape(0) + input_size = np.prod(input_shape) + output_shape = self.context.get_binding_shape(1) + output_size = np.prod(output_shape) + + d_input = cuda.mem_alloc(input_size * np.float32().nbytes) + d_output = cuda.mem_alloc(output_size * np.float32().nbytes) - # fixed allocation: does not account for multiple bindings/batch sizes (single input -> output tensor) - def allocate_buffers(self): - engine = self.engine + for _ in range(20): + random_input = np.random.randn(*input_shape).astype(np.float32) + cuda.memcpy_htod(d_input, random_input) + self.context.execute(bindings=[int(d_input), int(d_output)]) - self.input_shape = engine.get_binding_shape(0) - self.output_shape = engine.get_binding_shape(1) + self.get_logger().info(f"Engine warmed up with 20 inference passes.") + + def pointer_callback(self, msg): + # Convert the string back to bytes for the IPC handle + ipc_handle_str = msg.data + ipc_handle_bytes = bytes(int(ipc_handle_str[i:i+2], 16) for i in range(0, len(ipc_handle_str), 2)) - # Allocate device memory for input/output - self.d_input = cuda.mem_alloc(trt.volume(self.input_shape) * np.dtype(np.float32).itemsize) - self.d_output = cuda.mem_alloc(trt.volume(self.output_shape) * np.dtype(np.float32).itemsize) + # Recreate the IPC handle using PyCUDA + ipc_handle = cuda.IPCHandle(ipc_handle_bytes) - # Allocate host pinned memory for input/output (pinned memory for input/output buffers) - self.h_input = cuda.pagelocked_empty(trt.volume(self.input_shape), dtype=np.float32) - self.h_output = cuda.pagelocked_empty(trt.volume(self.output_shape), dtype=np.float32) + # Map the IPC memory into the current process + d_input = cuda.IPCMemoryHandle(ipc_handle) + + # Map the memory to the current context + self.d_input = d_input.open(cuda.Context.get_current()) + self.get_logger().info(f"Received IPC handle: {ipc_handle_str}") - # Create a CUDA stream for async execution - self.stream = cuda.Stream() - - def image_callback(self, msg): - self.arrival_time = Time.from_msg(msg.header.stamp) - image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8") - self.image = image - latency = self.get_clock().now() - Time.from_msg(msg.header.stamp) - self.get_logger().info(f"Latency: {latency.nanoseconds / 1e6} ms") - self.preprocess(image) - - def preprocess(self, image): - tic = time.perf_counter_ns() - # Preprocess the image (e.g. normalize) - input_data = image.astype(np.float32) - input_data = np.transpose(input_data, (2, 0, 1)) # HWC to CHW - input_data = np.expand_dims(input_data, axis=0) # add batch dimension - # Copy input data to pinned memory (host side) - np.copyto(self.h_input, input_data.ravel()) - toc = time.perf_counter_ns() - self.get_logger().info(f"Preprocessing: {(toc-tic)/1e6} ms") - self.run_inference() - def run_inference(self): tic = time.perf_counter_ns() cuda_driver_context.push() - # Transfer data from host to device asynchronously - cuda.memcpy_htod_async(self.d_input, self.h_input, self.stream) # Execute inference asynchronously self.exec_context.execute_async_v2(bindings=[int(self.d_input), int(self.d_output)], stream_handle=self.stream.handle) - # Transfer output data from device to host asynchronously - cuda.memcpy_dtoh_async(self.h_output, self.d_output, self.stream) - # Synchronize the stream to ensure the transfers are completed self.stream.synchronize() - # Return the output from host memory - output = np.copy(self.h_output) cuda_driver_context.pop() toc = time.perf_counter_ns() self.get_logger().info(f"Inference: {(toc-tic)/1e6} ms") - self.postprocess(output) - - # output shape: (1, 5, 8400) + + # # Assuming the result is in d_output, we could copy it back to host if needed + # result = np.empty(self.input_shape, dtype=np.float32) + # cuda.memcpy_dtoh(result, self.d_output) + # self.get_logger().info(f"Inference complete. Output shape: {result.shape}") + # Publish the IPC handles to postprocessing node + def postprocess(self, output): tic = time.perf_counter_ns() num_detections = len(output) // 5 - output = np.reshape(output, (num_detections, 5)) + output = np.reshape(output, (5, num_detections)) + # output = np.reshape(output, (num_detections, 5)) width = 640 height = 640 @@ -139,34 +111,43 @@ def postprocess(self, output): boxes = [] confidences = [] - for detection in output: - # print(detection) - obj_conf, x_center, y_center, bbox_width, bbox_height = detection[:] + # print("HERE!!!", output.shape) + # print(output[:,:20]) + + for i in range(output.shape[1]): + # for detection in output: + + detection = output[..., i] + + # obj_conf, x_center, y_center, bbox_width, bbox_height = detection[:] + x_center, y_center, bbox_width, bbox_height, obj_conf = detection[:] + # x_min, # Apply sigmoid to object confidence and class score - obj_conf = 1 / (1 + np.exp(-obj_conf)) # Sigmoid for object confidence + # obj_conf = 1 / (1 + np.exp(-obj_conf)) # Sigmoid for object confidence # Filter out weak predictions based on confidence threshold if obj_conf < conf_threshold: continue + print(detection) # Convert normalized values to absolute pixel values x_center_pixel = int(x_center) y_center_pixel = int(y_center) bbox_width_pixel = int(bbox_width) bbox_height_pixel = int(bbox_height) - print(f"[{obj_conf}, {x_center_pixel}, {y_center_pixel}, {bbox_width_pixel}, {bbox_height_pixel} ]") + # print(f"[{obj_conf}, {x_center_pixel}, {y_center_pixel}, {bbox_width_pixel}, {bbox_height_pixel} ]") # Calculate the top-left and bottom-right corners of the bounding box - # top_left_x = int(x_center_pixel - bbox_width_pixel / 2) - # top_left_y = int(y_center_pixel - bbox_height_pixel / 2) - # bottom_right_x = int(x_center_pixel + bbox_width_pixel / 2) - # bottom_right_y = int(y_center_pixel + bbox_height_pixel / 2) + top_left_x = int(x_center_pixel - bbox_width_pixel / 2) + top_left_y = int(y_center_pixel - bbox_height_pixel / 2) + bottom_right_x = int(x_center_pixel + bbox_width_pixel / 2) + bottom_right_y = int(y_center_pixel + bbox_height_pixel / 2) - boxes.append([x_center_pixel, y_center_pixel, bbox_width_pixel, bbox_height_pixel]) + # boxes.append([x_center_pixel, y_center_pixel, bbox_width_pixel, bbox_height_pixel]) # confidences.append(confidence) - # boxes.append([top_left_x, top_left_y, bottom_right_x, bottom_right_y]) + boxes.append([top_left_x, top_left_y, bottom_right_x, bottom_right_y]) # Append the box, confidence, and class score # boxes.append([x_min, y_min, x_max, y_max]) @@ -181,26 +162,32 @@ def postprocess(self, output): final_boxes.append([*boxes[i], confidences[i]]) toc = time.perf_counter_ns() self.get_logger().info(f"Postprocessing: {(toc-tic)/1e6} ms") - # self.display(final_boxes) - # print(final_boxes) self.display(final_boxes) + # print(final_boxes) + # self.display(boxes) + + # assign random tensor in cuda memory as dummy input for extermination node and try memory passing with the composition thing def display(self, final_boxes): image = self.image for box in final_boxes: x, y, w, h, confidence = box - cv2.rectangle(image, (x, y), (x + w, y + h), (255, 0, 0), 2) + cv2.rectangle(image, (x+h, y+w), (x-w, y-h), (255, 0, 0), 2) cv2.imshow("Image", image) cv2.waitKey(0) def main(args=None): rclpy.init(args=args) jetson_node = JetsonNode() - executor = MultiThreadedExecutor() + executor = MultiThreadedExecutor(num_threads=1) executor.add_node(jetson_node) - executor.spin() - jetson_node.destroy_node() - rclpy.shutdown() + + try: + executor.spin() + finally: + executor.shutdown() + jetson_node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': main() \ No newline at end of file