cagdas-aras.ciblak
/
Pong_Bot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

/LR_side_averages now publishes top and bottom points as well

This commit is contained in:
Cagdas Aras CIBLAK 2024-03-18 10:55:12 +01:00
parent 500e7d5d64
commit f016555ac0
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py
View File

@ -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()