diff --git a/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py b/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py index cf6ca3b..176f6dc 100755 --- a/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py +++ b/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py @@ -37,18 +37,24 @@ def sort_points_clockwise(centers): return sorted_centers def calculate_side_averages(sorted_centers): - """Calculate the average of the x and y coordinates for the left and right sides.""" + """Calculate the average of the x and y coordinates for all sides.""" # Assuming the sorted centers are in clockwise order: top-left, top-right, bottom-right, bottom-left right_average = np.mean([sorted_centers[1], sorted_centers[2]], axis=0) left_average = np.mean([sorted_centers[0], sorted_centers[3]], axis=0) - return left_average, right_average + top_average = np.mean([sorted_centers[0], sorted_centers[1]], axis=0) + bottom_average = np.mean([sorted_centers[2], sorted_centers[3]], axis=0) + return left_average, right_average, top_average, bottom_average -def publish_side_points(left_average, right_average): - """Publish the average points for the left and right sides with specified Z coordinates.""" +def publish_side_points(left_average, right_average, top_average, bottom_average): + """Publish the average points for all sides with specified Z coordinates.""" left_point = Point(x=left_average[0], y=left_average[1], z=1) # Z=1 for left right_point = Point(x=right_average[0], y=right_average[1], z=2) # Z=2 for right + top_point = Point(x=top_average[0], y=top_average[1], z=3) # Z=3 for top + bottom_point = Point(x=bottom_average[0], y=bottom_average[1], z=4) # Z=4 for bottom side_pub.publish(left_point) side_pub.publish(right_point) + side_pub.publish(top_point) + side_pub.publish(bottom_point) def image_callback(msg): global pub, marker_pub, side_pub @@ -70,9 +76,11 @@ def image_callback(msg): origin.z = 0 pub.publish(origin) - left_average, right_average = calculate_side_averages(sorted_centers) + left_average, right_average, top_average, bottom_average = calculate_side_averages(sorted_centers) publish_side_points(left_average - np.array([centerX, centerY]), - right_average - np.array([centerX, centerY])) + right_average - np.array([centerX, centerY]), + top_average - np.array([centerX, centerY]), + bottom_average - np.array([centerX, centerY])) # Publish the edges of the polygon marker = Marker()