From eb2feb67d9eef1a7d656ae6276ad00ae7ceb9f42 Mon Sep 17 00:00:00 2001
From: ros <louis.graz@ecam.fr>
Date: Wed, 13 Mar 2024 14:13:05 +0100
Subject: [PATCH] aruco_area now publishes polygon edges as well

---
 catkin_ws/src/ball_tracking/src/aruco_area.py | 41 ++++++++++++++++---
 1 file changed, 35 insertions(+), 6 deletions(-)

diff --git a/catkin_ws/src/ball_tracking/src/aruco_area.py b/catkin_ws/src/ball_tracking/src/aruco_area.py
index 30de56b..cbb1ed6 100755
--- a/catkin_ws/src/ball_tracking/src/aruco_area.py
+++ b/catkin_ws/src/ball_tracking/src/aruco_area.py
@@ -3,6 +3,8 @@
 import rospy
 from geometry_msgs.msg import Point
 from sensor_msgs.msg import Image
+from visualization_msgs.msg import Marker
+from std_msgs.msg import ColorRGBA
 import cv2
 import cv2.aruco as aruco
 import numpy as np
@@ -24,7 +26,7 @@ def find_polygon_centroid(centers):
     return centroid_x, centroid_y
 
 def image_callback(msg):
-    global pub
+    global pub, marker_pub
     bridge = CvBridge()
     frame = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
 
@@ -42,22 +44,49 @@ def image_callback(msg):
             origin.z = 0
             pub.publish(origin)
 
-            # Optional: Visualize the centroid for debugging
+            # Publish the edges of the polygon
+            marker = Marker()
+            marker.header.frame_id = "camera_link"  # or 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.lifetime = rospy.Duration()
+
+            # Add points for each marker center
+            for center in marker_centers:
+                p = Point()
+                p.x, p.y = center
+                p.z = 0  # Assuming a flat surface
+                marker.points.append(p)
+            
+            # Close the polygon by adding the first point again
+            if marker_centers:
+                p = Point()
+                p.x, p.y = marker_centers[0]
+                p.z = 0
+                marker.points.append(p)
+
+            marker_pub.publish(marker)
+
+            # Optional: Visualize the centroid and detected markers
             cv2.circle(frame, (int(centerX), int(centerY)), 5, (0, 255, 0), -1)
-            # Also, draw detected markers for visualization
             aruco.drawDetectedMarkers(frame, corners, ids)
 
-    # Display the frame with detected markers and the calculated centroid
+    # Display the frame
     cv2.imshow('Frame', frame)
     cv2.waitKey(1)
 
 def main():
-    global pub
+    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)
     
     # Initialize the ArUco dictionary
-    global arucoDict
     arucoDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
 
     # Subscribe to the camera_image topic