Joe vision attempt two

scripts/setup.bash

@@ -269,3 +269,5 @@ torpedo() {
 
 	ros2 service call /torpedo subjugator_msgs/srv/Servo "{angle: '$1'}"
 }
+
+export ROS_DOMAIN_ID=37

src/subjugator/gnc/subjugator_centroids/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>subjugator_centroids</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nottellingu@gmail.com">jh</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <depend>subjugator_msgs</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/subjugator/gnc/subjugator_centroids/random_code/green_tracker_cv2_example.py

@@ -0,0 +1,91 @@
+# this file isn't used and it's just an example of how to use cv2 to find centroids
+
+import cv2
+import numpy as np
+
+
+def get_lime_green_centroid(image):
+    """
+    Detect lime green areas in the image and return the centroid (x, y).
+    Returns None if no lime green region is detected.
+    """
+    # Convert BGR to HSV
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+
+    # Define HSV range for lime green
+    lower_green = np.array([40, 100, 100])
+    upper_green = np.array([80, 255, 255])
+
+    # Threshold the HSV image to get only green colors
+    mask = cv2.inRange(hsv, lower_green, upper_green)
+
+    # Find contours in the mask
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+    if not contours:
+        return None, mask
+
+    # Find the largest contour
+    largest = max(contours, key=cv2.contourArea)
+
+    # Compute centroid
+    M = cv2.moments(largest)
+    if M["m00"] == 0:
+        return None, mask
+
+    cx = int(M["m10"] / M["m00"])
+    cy = int(M["m01"] / M["m00"])
+    return (cx, cy), mask
+
+
+def display_debug_image(image, mask, centroid):
+    """
+    Displays the original image, the binary mask, and overlays the centroid (if found).
+    """
+    # Clone the original image
+    display_img = image.copy()
+
+    # Overlay centroid if it exists
+    if centroid:
+        cv2.circle(display_img, centroid, 5, (0, 0, 255), -1)
+        cv2.putText(
+            display_img,
+            f"Centroid: {centroid}",
+            (centroid[0] + 10, centroid[1]),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.5,
+            (0, 0, 255),
+            2,
+        )
+
+    # Show images
+    cv2.imshow("Lime Green Detection", display_img)
+    cv2.imshow("Mask", mask)
+
+
+def main():
+    cam_path = "/dev/v4l/by-id/usb-Chicony_Tech._Inc._Dell_Webcam_WB7022_4962D17A78D6-video-index0"
+    cap = cv2.VideoCapture(cam_path)
+
+    if not cap.isOpened():
+        print("Cannot open camera")
+        return
+
+    while True:
+        ret, frame = cap.read()
+        if not ret:
+            print("Failed to grab frame")
+            break
+
+        centroid, mask = get_lime_green_centroid(frame)
+        display_debug_image(frame, mask, centroid)
+
+        if cv2.waitKey(1) & 0xFF == ord("q"):
+            break
+
+    cap.release()
+    cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    main()

src/subjugator/gnc/subjugator_centroids/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/subjugator_centroids
+[install]
+install_scripts=$base/lib/subjugator_centroids

src/subjugator/gnc/subjugator_centroids/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'subjugator_centroids'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jh',
+    maintainer_email='nottellingu@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'subjugator_centroids = subjugator_centroids.subjugator_centroids_node:main'
+        ],
+    },
+)

src/subjugator/gnc/subjugator_centroids/subjugator_centroids/centroid_finder.py

@@ -0,0 +1,19 @@
+from abc import ABC, abstractmethod
+
+from cv2.typing import MatLike
+
+"""
+If you want to track some object, you need a class that implements this class
+"""
+
+
+class CentroidFinder(ABC):
+    @property
+    @abstractmethod
+    def topic_name(self) -> str:
+        pass
+
+    # returns (x, y) pair of centroid or None if not found
+    @abstractmethod
+    def find_centroid(self, frame: MatLike) -> tuple[int, int] | None:
+        pass

src/subjugator/gnc/subjugator_centroids/subjugator_centroids/green_tracker.py

@@ -0,0 +1,51 @@
+import cv2
+import numpy as np
+from cv2.typing import MatLike
+
+from subjugator_centroids.centroid_finder import CentroidFinder
+
+
+# example implementation of centroid abstract class, this one tracks green objects
+class GreenTracker(CentroidFinder):
+    def __init__(self, topic_name: str):
+        self.topic_name_: str = topic_name
+
+    @property
+    def topic_name(self) -> str:
+        return self.topic_name_
+
+    def find_centroid(self, frame: MatLike) -> tuple[int, int] | None:
+        """
+        Detect lime green areas in the image and return the centroid (x, y).
+        Returns None if no lime green region is detected.
+        """
+        # Convert BGR to HSV
+        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+
+        # Define HSV range for lime green
+        lower_green = np.array([40, 100, 100])
+        upper_green = np.array([80, 255, 255])
+
+        # Threshold the HSV image to get only green colors
+        mask = cv2.inRange(hsv, lower_green, upper_green)
+
+        # Find contours in the mask
+        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+        if not contours:
+            return None
+
+        # Find the largest contour (ignore small ones)
+        largest = max(contours, key=cv2.contourArea)
+        if cv2.contourArea(largest) < 50:
+            return None
+
+        # Compute centroid
+        M = cv2.moments(largest)
+        if M["m00"] == 0:
+            return None
+
+        cx = int(M["m10"] / M["m00"])
+        cy = int(M["m01"] / M["m00"])
+
+        return (cx, cy)

src/subjugator/gnc/subjugator_centroids/subjugator_centroids/red_tracker.py

@@ -0,0 +1,74 @@
+import cv2
+import numpy as np
+from cv2.typing import MatLike
+
+from subjugator_centroids.centroid_finder import CentroidFinder
+
+
+# example implementation of centroid abstract class, this one tracks green objects
+class RedTracker(CentroidFinder):
+    def __init__(self, topic_name: str):
+        self.topic_name_: str = topic_name
+        self.debug = False
+
+    @property
+    def topic_name(self) -> str:
+        return self.topic_name_
+
+    def find_centroid(self, frame: MatLike) -> tuple[int, int] | None:
+        """
+        Detect red areas in the image and return the centroid (x, y).
+        Returns None if no red region is detected.
+        """
+        # Convert BGR to HSV
+        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+
+        # Define HSV ranges for red (wraps around the 0° hue boundary)
+        lower_red1 = np.array([0, 140, 140])
+        upper_red1 = np.array([8, 255, 255])
+
+        lower_red2 = np.array([172, 140, 140])
+        upper_red2 = np.array([180, 255, 255])
+
+        # Create two masks and combine them
+        mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
+        mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
+        mask = cv2.bitwise_or(mask1, mask2)
+
+        if self.debug:
+            cv2.imshow("Red Mask", mask)
+            cv2.waitKey(1)
+
+        # Find contours in the mask
+        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+        if not contours:
+            return None
+
+        # Find the largest contour (ignore small ones)
+        largest = max(contours, key=cv2.contourArea)
+        if cv2.contourArea(largest) < 50:
+            return None
+
+        # Compute centroid
+        M = cv2.moments(largest)
+        if M["m00"] == 0:
+            return None
+
+        cx = int(M["m10"] / M["m00"])
+        cy = int(M["m01"] / M["m00"])
+
+        return (cx, cy)
+
+def test():
+    gt = RedTracker("testing/rn/sry")
+    gt.debug = True
+    cap =cv2.VideoCapture(0)
+    while True:
+        ret, frame = cap.read()
+        if not ret:
+            print("i hate you")
+        gt.find_centroid(frame)
+
+if __name__ == "__main__":
+    test()