diff --git a/ros2_ws/README.md b/ros2_ws/README.md index 6271723..a6cce21 100644 --- a/ros2_ws/README.md +++ b/ros2_ws/README.md @@ -29,7 +29,7 @@ To set up and run this workspace, you have several options: ### With sample images > **Warning:** The paths mentioned in the instructions are absolute paths based on the Jetson's directory. These arguments can be changed to be relative to your current directory. -1. Navigate to the `/ROS/workspace_python/ros2_ws` directory. +1. Navigate to the `/ROS/ros2_ws` directory. 2. Create temporary variables to store the paths values: ```bash IMAGE_FOLDER_PATH=/home/user/ROS/assets/maize @@ -41,28 +41,41 @@ To set up and run this workspace, you have several options: rosdep install --from-paths src --ignore-src -r -y # Build the workspace - colcon build --symlink-install --packages-select custom_interface python_package + colcon build --symlink-install --packages-select custom_interfaces python_package # why --symlink install: https://answers.ros.org/question/371822/what-is-the-use-of-symlink-install-in-ros2-colcon-build/ ``` -4. Run the `picture_node`: +4. Run the `picture_node` or `video_node`: ```bash source install/setup.bash ros2 run python_package picture_node --ros-args -p static_image_path:=$IMAGE_FOLDER_PATH -p frame_rate:=1 ``` + + For video node, run: + ``` + VIDEO_PATH=write/path/here + ros2 run python_package video_node --ros-args -p static_image_path:=$VIDEO_PATH -p frame_rate:=10 + ``` 5. Run the `inference_node` in a new terminal: ```bash MODEL_WEIGHT_PATH=/home/user/ROS/models/maize/Maize.pt source install/setup.bash - ros2 run python_package inference_node --ros-args -p weights_path:=$MODEL_WEIGHT_PATH + ros2 run python_package inference_node --ros-args -p weights_path:=$MODEL_WEIGHT_PATH ``` 6. Run the `extermination_node` in a new terminal: ```bash source install/setup.bash - ros2 run python_package extermination_node + ros2 run python_package extermination_node --ros-args -p use_display_node:=False ``` +### Getting the plant count using the service +#### Prerequisites: +- Extermination node must be active +```bash +source install/setup.bash +ros2 service call /reset_tracker custom_interfaces/srv/GetRowPlantCount "{}" +``` ## Common trouble shooting Some common troubleshooting steps include: diff --git a/ros2_ws/src/python_package/README.md b/ros2_ws/src/python_package/README.md index cbf2ae0..ee42a2e 100644 --- a/ros2_ws/src/python_package/README.md +++ b/ros2_ws/src/python_package/README.md @@ -39,7 +39,7 @@ Other nodes are still a WIP ## Additional information ### Compiling new Changes ```bash -colcon build --symlink-install --packages-select custom_interface python_package +colcon build --symlink-install --packages-select custom_interfaces python_package source install/setup.bash ``` diff --git a/ros2_ws/src/python_package/python_package/extermination_node.py b/ros2_ws/src/python_package/python_package/extermination_node.py index b030afd..4de8c94 100644 --- a/ros2_ws/src/python_package/python_package/extermination_node.py +++ b/ros2_ws/src/python_package/python_package/extermination_node.py @@ -1,6 +1,4 @@ -import time, os import cv2 -import serial # import pycuda.driver as cuda # from tracker import * # depth point cloud here... @@ -25,7 +23,7 @@ class ExterminationNode(Node): def __init__(self): super().__init__('extermination_node') - self.declare_parameter('use_display_node', True) + self.declare_parameter('use_display_node', False) self.declare_parameter('camera_side', 'left') self.use_display_node = self.get_parameter('use_display_node').get_parameter_value().bool_value @@ -39,7 +37,7 @@ def __init__(self): self.minimum_confidence = 0.5 self.boxes_present = 0 - self.tracker = EuclideanDistTracker() #value sent to arduino + self.tracker = EuclideanDistTracker() self.bridge = CvBridge() self.boxes_msg = Int8() self.boxes_msg.data = 0 @@ -57,7 +55,7 @@ def get_tracker_row_count_callback(self,request,response): return the tracker's current row count """ row_count = self.tracker.reset() - response.row_count = row_count + response.plant_count = row_count return response def inference_callback(self, msg): @@ -71,13 +69,12 @@ def inference_callback(self, msg): bounding_boxes = postprocess(confidence,bboxes, raw_image,msg.velocity) + bbox_with_label = self.tracker.update(bounding_boxes) - final_image = draw_boxes(raw_image,bounding_boxes,velocity=msg.velocity) + final_image = draw_boxes(raw_image,bbox_with_label,velocity=msg.velocity) - self.tracker.update(bounding_boxes) if self.use_display_node: - # Create a CUDA window and display the cv_image cv2.imshow(self.window, final_image) cv2.waitKey(10) @@ -90,7 +87,6 @@ def inference_callback(self, msg): self.boxes_msg.data = self.boxes_present def timer_callback(self): - #send msg to arduino self.box_publisher.publish(self.boxes_msg) self.get_logger().info("Published results to Proxy Node") diff --git a/ros2_ws/src/python_package/python_package/picture_node.py b/ros2_ws/src/python_package/python_package/picture_node.py index 46230ef..43d0c5e 100644 --- a/ros2_ws/src/python_package/python_package/picture_node.py +++ b/ros2_ws/src/python_package/python_package/picture_node.py @@ -1,4 +1,4 @@ -import time, os +import os import cv2 import random import rclpy @@ -6,7 +6,7 @@ from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from sensor_msgs.msg import Image -from std_msgs.msg import Header, String +from std_msgs.msg import Header from cv_bridge import CvBridge import queue import numpy as np @@ -57,6 +57,8 @@ def get_images(self)-> list[np.ndarray]: 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}") + filename = self.static_image_path + image_paths = [] if os.path.isfile(self.static_image_path) \ and (filename.endswith('.JPG') or filename.endswith('.png')): @@ -114,7 +116,7 @@ def publish_static_image(self): # publish image and increment whatever is needed self.input_image_publisher.publish(image_input) - + self.get_logger().info(f"Published image {self.image_counter}") self.image_counter += 1 self.loop_count = self.image_counter // array_size else: diff --git a/ros2_ws/src/python_package/python_package/scripts/tracker.py b/ros2_ws/src/python_package/python_package/scripts/tracker.py index da702e2..f783cf0 100644 --- a/ros2_ws/src/python_package/python_package/scripts/tracker.py +++ b/ros2_ws/src/python_package/python_package/scripts/tracker.py @@ -1,12 +1,30 @@ import math +THRESHOLD = 200 class EuclideanDistTracker: - def __init__(self): + """ + source: https://www.analyticsvidhya.com/blog/2022/05/a-tutorial-on-centroid-tracker-counter-system/ + Object tracking using euclidean distance. TLDR: + - keep list of obj center points from the previous frame + - compare the center points of the current frame to the previous frame + - if the distance between the center points is less than a threshold, then it is the same object + - if the distance is greater than the threshold, then it is a new object + + Current issues/limitations: + - Need to define our own threshold value, which is somewhat dependant on the bot speed and camera frame rate + - Dependent on proper object detection - if the obj detection fails the previous frame, + the tracker will see everything as a new object + - If two detections are close to each other, tracker will assign them the same object ID. + A "fix" right now is to keep track of previous IDs that we "used", so we don't assign them again. + """ + + def __init__(self, threshold=THRESHOLD): # Store the center positions of the objects self.center_points = {} # Keep the count of the IDs # each time a new object id detected, the count will increase by one self.id_count = 0 + self.threshold = threshold def reset(self): """ resets the count and also returns val of curr row @@ -20,6 +38,7 @@ def reset(self): def update(self, objects_rect): # Objects boxes and ids objects_bbs_ids = [] + centers = self.center_points.copy() # Get center point of new object for rect in objects_rect: @@ -29,12 +48,13 @@ def update(self, objects_rect): # Find out if that object was detected already same_object_detected = False - for item_id, pt in self.center_points.items(): + for item_id, pt in centers.items(): dist = math.hypot(cx - pt[0], cy - pt[1]) - if dist < 200: + if dist < self.threshold: self.center_points[item_id] = (cx, cy) objects_bbs_ids.append([x1, y1, x2, y2, item_id]) same_object_detected = True + centers.pop(item_id) break # New object is detected we assign the ID to that object diff --git a/ros2_ws/src/python_package/python_package/scripts/utils.py b/ros2_ws/src/python_package/python_package/scripts/utils.py index f57f1b2..b7fb9a0 100644 --- a/ros2_ws/src/python_package/python_package/scripts/utils.py +++ b/ros2_ws/src/python_package/python_package/scripts/utils.py @@ -28,8 +28,24 @@ def initialise_model(weights_path=None, precision=None): yolo = YOLO(weights_path) return yolo +def _resize_with_padding(image: np.ndarray): + """ + Resize the image to the desired shape and pad the image with a constant color. + """ + new_shape = POSTPROCESS_OUTPUT_SHAPE + padding_color = (0, 255, 255) + original_shape = (image.shape[1], image.shape[0]) + ratio = float(max(new_shape))/max(original_shape) + new_size = tuple([int(x*ratio) for x in original_shape]) + image = cv2.resize(image, new_size) + delta_w = new_shape[0] - new_size[0] + delta_h = new_shape[1] - new_size[1] + top, bottom = delta_h//2, delta_h-(delta_h//2) + left, right = delta_w//2, delta_w-(delta_w//2) + preprocessed_img = cv2.copyMakeBorder(image, top, bottom, left, right, cv2.BORDER_CONSTANT, value=padding_color) + + return preprocessed_img -# Not used for now def preprocess(image: np.ndarray): """ Takes in a numpy array that has been preprocessed @@ -85,8 +101,8 @@ def postprocess(confidence, bbox_array: np.ndarray,raw_image: np.ndarray, veloci """ detections = _convert_bboxes_to_pixel(bbox_array, raw_image.shape) - detections = _object_filter(raw_image, detections) #color segmentation - detections = _verify_object(raw_image, detections,velocity) + # detections = _object_filter(raw_image, detections) #color segmentation + # detections = _verify_object(raw_image, detections,velocity) return detections @@ -115,7 +131,6 @@ def _convert_bboxes_to_pixel(bbox_array: np.ndarray, image_shape: Tuple[int, int def _object_filter(image: np.ndarray, bboxes: List[Tuple[int, int, int, int]]) -> List[Tuple[int, int, int, int]]: """ Filters objects in an image based on bounding boxes and color thresholds. - Args: image (np.ndarray): The input image in which objects are to be filtered. bboxes (List[Tuple[int, int, int, int]]): A list of bounding boxes. @@ -189,7 +204,7 @@ def _verify_object(raw_image, bboxes, velocity=0): return adjusted_bboxes -def draw_boxes(image: np.ndarray, bboxes: list, with_roi =True, with_roi_shift = True, velocity = 0) -> np.ndarray: +def draw_boxes(image: np.ndarray, bboxes: list, with_roi =False, with_roi_shift = False, velocity = 0) -> np.ndarray: """ Given array of bounding box tuples and an image, draw the bounding boxes into the image. If with_roi and with_roi shift is set to true, the ROI areas will also be drawn in. @@ -225,10 +240,15 @@ def draw_boxes(image: np.ndarray, bboxes: list, with_roi =True, with_roi_shift = color = tuple(np.random.randint(0, 256, 3).tolist()) # Generate a random color for bbox in bboxes: - x1, y1, x2, y2 = map(int, bbox) + x1, y1, x2, y2, label = map(int, bbox) - # print(f"Bounding box: ({x1}, {y1}), ({x2}, {y2})") - image = cv2.rectangle(image, (x1, y1), (x2, y2),(255, 0, 0), 2) + + image = cv2.rectangle(image, (x1, y1), (x2, y2), (255, 0, 0), 2) + label = f"Object {label}" + cv2.putText(image, label, (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1, cv2.LINE_AA) + + + image = _resize_with_padding(image) return image diff --git a/ros2_ws/src/python_package/python_package/video_node.py b/ros2_ws/src/python_package/python_package/video_node.py index e22d8d7..07007ea 100644 --- a/ros2_ws/src/python_package/python_package/video_node.py +++ b/ros2_ws/src/python_package/python_package/video_node.py @@ -1,144 +1,120 @@ -import time, os +import os import cv2 -import pycuda.driver as cuda -import cupy as cp -import queue - +import random 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 std_msgs.msg import Header from cv_bridge import CvBridge +from .scripts.utils import preprocess +from custom_interfaces.msg import ImageInput # CHANGE +# node to publish static image data to the /image topic. +# used for testing the inference pipelines. class VideoNode(Node): def __init__(self): super().__init__('video_node') - cuda.init() - device = cuda.Device(0) - self.cuda_driver_context = device.make_context() - self.stream = cuda.Stream() - - 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('shift_constant', 1) - self.declare_parameter('roi_dimensions', [0, 0, 100, 100]) - self.declare_paramter('precision', 'fp32') - + self.declare_parameter('video_path', '/home/user/ROS/assets/test_videos/IMG_5947.MOV') + self.declare_parameter('loop', -1) # 0 = don't loop, >0 = # of loops, -1 = loop forever + self.declare_parameter('frame_rate', 10) # Desired frame rate for publishing + self.declare_parameter('camera_side', 'left') + self.camera_side = self.get_parameter('camera_side').get_parameter_value().string_value + self.video_path = self.get_parameter('video_path').get_parameter_value().string_value + # Resolve the path programmatically + if not os.path.isabs(self.video_path): + self.video_path = os.path.join(os.getcwd(), self.video_path) + 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.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.precision = self.get_parameter('precision').get_parameter_value().string_value - self.pointer_publisher = self.create_publisher(String, 'preprocessing_done', 10) - self.image_service = self.create_service(Image, 'image_data', self.image_callback) - - self.velocity = [0, 0, 0] - self.image_queue = queue.Queue() self.bridge = CvBridge() - self.publish_video_frames() - # propagate fp16 option (.fp16 datatype for cupy) - if self.precision == 'fp32': - pass - elif self.precision == 'fp16': - pass - else: - self.get_logger().error(f"Invalid precision: {self.precision}") - - def image_callback(self, response): - self.get_logger().info("Received image request") - if not self.image_queue.empty(): - image_data = self.image_queue.get() # Get the image from the queue - cv_image, velocity = image_data # unpack tuple (image, velocity) - - # Convert OpenCV image to ROS2 Image message using cv_bridge - ros_image = self.bridge.cv2_to_imgmsg(cv_image, encoding='rgb8') - # Create a new header and include velocity in the stamp fields - header = Header() - current_time = self.get_clock().now().to_msg() - header.stamp = current_time # Set timestamp to current time - header.frame_id = str(velocity) # Set frame ID to velocity - - ros_image.header = header # Attach the header to the ROS image message - response.image = ros_image # Set the response's image - response.image = ros_image # Set the response's image - return response + self.loop_count = 0 + self.image_counter = 0 + + + self.openVR() + + video_fps = self.vr.get(cv2.CAP_PROP_FPS) + self.interval = int(video_fps / self.frame_rate) + + self.counter = 0 + + self.input_image_publisher = self.create_publisher(ImageInput, f'{self.camera_side}_image_input', 10) + timer_period = 1/self.frame_rate # publish every 0.5 seconds + self.timer = self.create_timer(timer_period * 2, self.publish_static_image) + + + + def publish_static_image(self): + """ + Publishes static images to the /image topic + """ + count = 0 + interval = self.interval + + success, image = self.vr.read() + while success: + if count == interval: + image_input = ImageInput() + + raw_image = image + postprocessed_img = preprocess(raw_image) + postprocessed_img_msg = self.bridge.cv2_to_imgmsg(postprocessed_img, encoding='rgb8') + raw_img_msg = self.bridge.cv2_to_imgmsg(raw_image, encoding='rgb8') + + image_input.header = Header() + image_input.header.frame_id = 'static_image' + image_input.raw_image = raw_img_msg + image_input.preprocessed_image = postprocessed_img_msg + image_input.velocity = random.uniform(0,1) + + + + + # publish image and increment whatever is needed + self.input_image_publisher.publish(image_input) + self.get_logger().info(f"Published image {self.image_counter}") + self.image_counter += 1 + + return + + success, image = self.vr.read() + count += 1 + + if self.loop == -1 or self.loop_count < self.loop: + self.openVR() + self.loop_count += 1 + self.publish_static_image() else: - self.get_logger().error("Image queue is empty") - return response - - def publish_video_frames(self): - if not os.path.exists(self.video_path): - self.get_logger().error(f"Video file not found at {self.video_path}") - return - - cap = cv2.VideoCapture(self.video_path) - if not cap.isOpened(): - self.get_logger().error(f"Failed to open video: {self.video_path}") - return - - loops = 0 - while rclpy.ok() and (self.loop == -1 or loops < self.loop): - while cap.isOpened() and rclpy.ok(): - ret, frame = cap.read() - if not ret: - break - self.image_queue.put((frame, self.velocity[0])) - self.preprocess_image(frame) - time.sleep(1.0 / self.frame_rate) + # stop the timer/quit the node + self.timer.cancel() + self.get_logger().info("Finished publishing images") + self.destroy_node() + - if self.loop > 0: - loops += 1 - if self.loop != -1: - cap.set(cv2.CAP_PROP_POS_FRAMES, 0) # restart video - cap.release() - - 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) # Now the image is on GPU memory as CuPy array - input_data = input_data.astype(cp.float32) / 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) - input_data = cp.ravel(input_data) # flatten the array - - 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 openVR(self): + self.vr = cv2.VideoCapture(self.video_path) + if not self.vr.isOpened(): + print("Error: Could not open video.") + self.timer.cancel() + self.destroy_node() def main(args=None): rclpy.init(args=args) video_node = VideoNode() executor = MultiThreadedExecutor(num_threads=1) executor.add_node(video_node) - + try: executor.spin() + except KeyboardInterrupt: + print("Shutting down video node") + return finally: executor.shutdown() video_node.destroy_node() diff --git a/ros2_ws/src/python_package/setup.py b/ros2_ws/src/python_package/setup.py index 17a70fa..5c03b6a 100644 --- a/ros2_ws/src/python_package/setup.py +++ b/ros2_ws/src/python_package/setup.py @@ -26,7 +26,8 @@ 'inference_node = python_package.inference_node:main', 'extermination_node = python_package.extermination_node:main', 'proxy_node = python_package.proxy_node:main', - 'picture_node = python_package.picture_node:main' # remove later + 'picture_node = python_package.picture_node:main', #remove later + 'video_node = python_package.video_node:main' ], }, diff --git a/ros2_ws/src/python_package/test/test_comparison.py b/ros2_ws/src/python_package/test/test_comparison.py index 1856a7a..54ec9df 100644 --- a/ros2_ws/src/python_package/test/test_comparison.py +++ b/ros2_ws/src/python_package/test/test_comparison.py @@ -5,9 +5,9 @@ import cupy as cp import time from pathlib import Path -from python_workspace.scripts.utils import ModelInference -from python_workspace.scripts.utils_cupy import ModelInferenceCupy -from python_workspace.scripts.utils_pytorch import ModelInferencePytorch +from python_package.scripts.utils import ModelInference +from python_package.scripts.utils_cupy import ModelInferenceCupy +from python_package.scripts.utils_pytorch import ModelInferencePytorch class TestPreprocessingMethods(unittest.TestCase): @classmethod diff --git a/ros2_ws/src/python_package/test/test_utils.py b/ros2_ws/src/python_package/test/test_utils.py index 5f7f0bd..a9db652 100644 --- a/ros2_ws/src/python_package/test/test_utils.py +++ b/ros2_ws/src/python_package/test/test_utils.py @@ -3,7 +3,7 @@ import numpy as np import os from pathlib import Path -from python_workspace.scripts.utils import ModelInference +from python_package.scripts.utils import ModelInference class TestModelInference(unittest.TestCase): @classmethod @@ -124,4 +124,4 @@ def calculate_iou(box1, box2): if __name__ == '__main__': unittest.main() -# python3 workspace_python/ros2_ws/src/python_workspace/test/test_utils.py \ No newline at end of file +# python3 workspace_python/ros2_ws/src/python_package/test/test_utils.py \ No newline at end of file