Feature/25 mvp integration

workspace_python/.gitignore

@@ -0,0 +1,3 @@
+/build/
+/install/
+/log/

workspace_python/ros2_ws/.gitignore

@@ -1,4 +1,7 @@
-build/
-install/
-log/
-*.onnx
+build/
+install/
+log/
+datasets/
+
+*.onnx
+*.pt

workspace_python/ros2_ws/src/python_workspace/README.md

@@ -1,6 +1,39 @@
-Running the camera node:
-ros2 run python_workspace camera_node --ros-args -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]
-
-ros2 run python_workspace jetson_node
-
-ros2 run python_workspace extermination_node
+
+### List of Nodes
+
+| Node Type | Subscribes To | Publishes To | Example command |
+|-------------------|----------------|-----------------------------------|------|
+| Picture Node | `/picture/command` | `/input_image` | `ros2 run python_workspace picture_node --ros-args -p static_image_path:='./../assets/maize' -p loop:=-1 -p frame_rate:=1`|
+| Inference Node | `/input_image` | -`/bounding_box_coordinates` <br> - `/output_img` | `ros2 run python_workspace inference_node --ros-args -p weights_path:='./ros2_ws/src/python_workspace/python_workspace/scripts/yolo11n.pt'`|
+
+
+### List of Topics and Data Types
+
+| Topic Name | Description | Data Type |
+|-----------------------------|--------------------------------------|--------------------|
+| `/input_image` | Input image for processing | `sensor_msgs/Image`|
+| `/bounding_box_coordinates` | Coordinates of detected objects (xyxy) | `std_msgs/Float32MultiArray` |
+| `/output_img` | Processed output image | `sensor_msgs/Image`|
+
+
+
+### Other commands
+#### Running the camera node:
+`ros2 run python_workspace camera_node --ros-args -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]`
+
+#### Running the jetson_node
+ros2 run python_workspace jetson_node
+
+#### Running the extermination node
+ros2 run python_workspace extermination_node
+#### Running the picture node:
+
+`ros2 run python_workspace picture_node --ros-args -p static_image_path:='./../assets/maize' -p loop:=-1 -p frame_rate:=1`
+
+#### Running the inference node:
+
+The path for weights (.pt) is relative to the ROS/workspace_python/ros2_ws directory. 
+
+```bash
+ros2 run python_workspace inference_node --ros-args -p weights_path:='./ros2_ws/src/python_workspace/python_workspace/scripts/yolo11n.pt'
+```

workspace_python/ros2_ws/src/python_workspace/python_workspace/inference_node.py

@@ -0,0 +1,65 @@
+import os
+from cv_bridge import CvBridge
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from std_msgs.msg import Int32MultiArray, Float32MultiArray
+
+from .scripts.utils import ModelInference
+
+class InferenceNode(Node):
+ def __init__(self):
+ super().__init__('inference_node')
+
+ self.declare_parameter('weights_path', './src/python_workspace/python_workspace/scripts/best.onnx')
+ self.declare_parameter('precision', 'fp32') # fp32, fp16 # todo: do something with strip_weights and precision
+
+ self.weights_path = self.get_parameter('weights_path').get_parameter_value().string_value
+ if not os.path.isabs(self.weights_path):
+ self.weights_path = os.path.join(os.getcwd(), self.weights_path)
+
+
+ self.precision = self.get_parameter('precision').get_parameter_value().string_value
+
+ # instantiate the model here
+ self.model = ModelInference(self.weights_path, self.precision)
+
+ self.bridge = CvBridge()
+
+ self.subscription = self.create_subscription(Image, 'input_image', self.image_callback, 10)
+
+ # create a publisher for the output image/boxes/extermination data
+ self.box_publisher = self.create_publisher(Float32MultiArray,'bounding_box_coordinates', 10)
+
+ self.output_image_publisher = self.create_publisher(Image, 'output_img', 10)
+
+
+ def image_callback(self, msg):
+ # print("============================================")
+ opencv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
+ output_img, named_classes, confidences, boxes = self.model.inference(opencv_img)
+
+ output_msg = Float32MultiArray()
+ output_msg.data = []
+ if len(boxes) != 0:
+ output_msg.data = boxes
+ self.box_publisher.publish(output_msg)
+
+
+ # convert the output image to a ROS2 image message
+ output_msg = self.bridge.cv2_to_imgmsg(output_img, encoding='rgb8')
+
+ self.output_image_publisher.publish(output_msg)
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ inference_node = InferenceNode()
+ rclpy.spin(inference_node)
+ inference_node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()

workspace_python/ros2_ws/src/python_workspace/python_workspace/picture_node.py

@@ -1,8 +1,5 @@
 import time, os
 import cv2
-import pycuda.driver as cuda
-import cupy as cp
-import queue
 
 import rclpy
 from rclpy.time import Time
@@ -12,130 +9,93 @@
 from std_msgs.msg import Header, String
 from cv_bridge import CvBridge
 
+
+# node to publish static image data to the /image topic. 
+# used for testing the inference pipelines.
 class PictureNode(Node):
  def __init__(self):
  super().__init__('picture_node')
-
- cuda.init()
- device = cuda.Device(0)
- self.cuda_driver_context = device.make_context()
- self.stream = cuda.Stream()
 
  self.declare_parameter('static_image_path', '/home/usr/Desktop/ROS/assets/IMG_1822_14.JPG')
  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('frame_rate', 1) # Desired frame rate for publishing
+
 
  self.static_image_path = self.get_parameter('static_image_path').get_parameter_value().string_value
+ # Resolve the path programmatically
+ if not os.path.isabs(self.static_image_path):
+ self.static_image_path = os.path.join(os.getcwd(), self.static_image_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.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.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_static_image()
- # 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
-
- else:
- self.get_logger().error("Image queue is empty")
- return response
-
- def publish_static_image(self):
+
+ self.image_list = self.get_images()
+ self.loop_count = 0
+ self.image_counter = 0
+
+ self.input_image_publisher = self.create_publisher(Image, 'input_image', 10)
+ timer_period = 1/self.frame_rate # publish every 0.5 seconds
+ self.timer = self.create_timer(timer_period, self.publish_static_image)
+
+
+ def get_images(self):
+ """
+ Returns a list of images in the form of ROS2 image messages from the path specified by the static_image_path parameter.
+ """
+ # You can technically read the img file as binary data rather than using cv2 to read it and then convert it into a ROS2 image message... and then converting it back to a numpy array in the inference node. But since the picture node is only for testing the MVP and we're not using the GPU yet the overhead here should not matter.
  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
+ raise FileNotFoundError(f"Static image not found at {self.static_image_path}")
 
- images = []
- if os.path.isdir(self.static_image_path):
+ image_paths = []
+ if os.path.isfile(self.static_image_path) \
+ and (filename.endswith('.JPG') or filename.endswith('.png')):
+ filename = os.path.join(self.static_image_path, filename)
+ image_paths.append(filename)
+ elif 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:
+ filename = os.path.join(self.static_image_path, filename)
+ image_paths.append(filename)
+
+ if len(image_paths) == 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 images:
- image = cv2.imread(filename, cv2.IMREAD_COLOR)
- if image is not None:
- self.image_queue.put((image, self.velocity[0]))
- 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
-
- 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
+ images = []
+ for filename in image_paths:
+
+ image = cv2.imread(filename, cv2.IMREAD_COLOR)
+ ros_image = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
+ if image is None:
+ self.get_logger().error(f"Failed to read image: {filename}")
+ raise FileNotFoundError(f"Failed to read image: {filename}")
+ images.append(ros_image)
+
+ return images
 
- 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")
+ def publish_static_image(self):
+ """
+ Publishes static images to the /image topic
+ """
+ array_size = len(self.image_list)
+
+ # publish the image on the current pos to the image topic
+ if self.loop == -1 or self.loop_count < self.loop:
+ position = self.image_counter % array_size
+ self.get_logger().info(f"Publishing image {position + 1}/{array_size}")
+ self.input_image_publisher.publish(self.image_list[position])
 
- # 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)
+ self.image_counter += 1
+ self.loop_count = self.image_counter // array_size
+ else:
+ # stop the timer/quit the node
+ self.timer.cancel()
+ self.get_logger().info("Finished publishing images")
+ self.destroy_node()
 
 def main(args=None):
  rclpy.init(args=args)