Polygon testing script added along with bug fixes to edge detection

catkin_ws/src/ball_tracking/src/aruco_area.py

@@ -25,6 +25,17 @@ def find_polygon_centroid(centers):
     centroid_y = np.mean([center[1] for center in centers], axis=0)
     return centroid_x, centroid_y
 
+def sort_points_clockwise(centers):
+    """Sort the points in a clockwise order based on their angles relative to the centroid."""
+    if not centers:
+        return []
+
+    centroid = np.mean(centers, axis=0)
+    def angle_from_centroid(center):
+        return np.arctan2(center[1] - centroid[1], center[0] - centroid[0])
+    sorted_centers = sorted(centers, key=angle_from_centroid)
+    return sorted_centers
+
 def image_callback(msg):
     global pub, marker_pub
     bridge = CvBridge()
@@ -33,9 +44,10 @@ def image_callback(msg):
     gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
     corners, ids, rejected = aruco.detectMarkers(gray, arucoDict)
 
-    if ids is not None and len(ids) == 4:  # Ensure exactly four markers are detected
+    if ids is not None and len(ids) >= 4:  # Adjust this condition based on your setup
         marker_centers = calculate_marker_centers(corners)
-        centerX, centerY = find_polygon_centroid(marker_centers)
+        sorted_centers = sort_points_clockwise(marker_centers)
+        centerX, centerY = find_polygon_centroid(sorted_centers)
 
         if centerX is not None and centerY is not None:
             origin = Point()
@@ -46,27 +58,27 @@ def image_callback(msg):
 
             # Publish the edges of the polygon
             marker = Marker()
-            marker.header.frame_id = "camera_link"  # or your camera frame
+            marker.header.frame_id = "camera_link"  # Adjust to your camera frame
             marker.header.stamp = rospy.Time.now()
             marker.ns = "polygon_edges"
             marker.id = 0
             marker.type = Marker.LINE_STRIP
             marker.action = Marker.ADD
-            marker.scale.x = 0.02  # Adjust the line width as needed
+            marker.scale.x = 0.02  # Line width
-            marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)  # Red line
+            marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)  # Red color
             marker.lifetime = rospy.Duration()
 
-            # Add points for each marker center
+            # Add points for each sorted marker center
-            for center in marker_centers:
+            for center in sorted_centers:
                 p = Point()
                 p.x, p.y = center
-                p.z = 0  # Assuming a flat surface
+                p.z = 0
                 marker.points.append(p)
             
             # Close the polygon by adding the first point again
-            if marker_centers:
+            if sorted_centers:
                 p = Point()
-                p.x, p.y = marker_centers[0]
+                p.x, p.y = sorted_centers[0]
                 p.z = 0
                 marker.points.append(p)
 
@@ -84,7 +96,7 @@ def main():
     global pub, marker_pub, arucoDict
     rospy.init_node('aruco_origin_publisher', anonymous=True)
     pub = rospy.Publisher('/aruco_origin', Point, queue_size=10)
-    marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
+    marker_pub = rospy.Publisher('/polygon_edges', Marker, queue_size=10)
     
     # Initialize the ArUco dictionary
     arucoDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

catkin_ws/src/ball_tracking/src/polygon_tester.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rospy
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point
+from visualization_msgs.msg import Marker
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import numpy as np
+
+class ArucoVisualizer:
+    def __init__(self):
+        self.bridge = CvBridge()
+        self.image_sub = rospy.Subscriber('/camera_image', Image, self.image_callback)
+        self.origin_sub = rospy.Subscriber('/aruco_origin', Point, self.origin_callback)
+        self.edges_sub = rospy.Subscriber('/polygon_edges', Marker, self.edges_callback)
+        
+        self.current_frame = None
+        self.origin_point = None
+        self.edge_points = []
+
+    def origin_callback(self, data):
+        """Callback function for the origin point."""
+        self.origin_point = (int(data.x), int(data.y))
+
+    def edges_callback(self, data):
+        """Callback function for the polygon edges."""
+        self.edge_points = []
+        if data.type == Marker.LINE_STRIP:
+            for p in data.points:
+                self.edge_points.append((int(p.x), int(p.y)))
+    
+    def image_callback(self, data):
+        """Callback function for camera image."""
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+            self.current_frame = cv_image
+            self.draw_visuals()
+        except CvBridgeError as e:
+            print(e)
+
+    def draw_visuals(self):
+        """Draw the origin point and polygon edges on the current frame."""
+        if self.current_frame is not None:
+            frame = self.current_frame.copy()
+            # Draw the origin
+            if self.origin_point is not None:
+                cv2.circle(frame, self.origin_point, 5, (0, 255, 0), -1)
+
+            # Draw the edges
+            if len(self.edge_points) > 1:
+                for i in range(len(self.edge_points) - 1):
+                    cv2.line(frame, self.edge_points[i], self.edge_points[i + 1], (0, 0, 255), 2)
+                # Optionally, close the polygon
+                cv2.line(frame, self.edge_points[-1], self.edge_points[0], (0, 0, 255), 2)
+
+            # Display the frame
+            cv2.imshow('Aruco Visualizer', frame)
+            cv2.waitKey(1)
+
+def main():
+    rospy.init_node('aruco_visualizer', anonymous=True)
+    av = ArucoVisualizer()
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down")
+    cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    main()
+