diff --git a/catkin_ws/src/ball_tracking/src/aruco_area.py b/catkin_ws/src/ball_tracking/src/aruco_area.py index cbb1ed6..ad4fbaf 100755 --- a/catkin_ws/src/ball_tracking/src/aruco_area.py +++ b/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.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # Red line + marker.scale.x = 0.02 # Line width + marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # Red color marker.lifetime = rospy.Duration() - # Add points for each marker center - for center in marker_centers: + # Add points for each sorted marker center + 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) diff --git a/catkin_ws/src/ball_tracking/src/polygon_tester.py b/catkin_ws/src/ball_tracking/src/polygon_tester.py new file mode 100755 index 0000000..fa2766c --- /dev/null +++ b/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() +