vis polygon

Author: KenC1014

GEMstack/onboard/perception/cone_detection_parking.py

@@ -336,6 +336,7 @@ def initialize(self):
         self.pub_cones_bboxes_markers = rospy.Publisher("cones_detection/bboxes/markers", MarkerArray, queue_size=10)
         self.pub_cones_centers_pc2 = rospy.Publisher("cones_detection/centers/point_cloud", PointCloud2, queue_size=10)
         self.pub_parking_spot_marker = rospy.Publisher("parking_spot_detection/marker", MarkerArray, queue_size=10)
+        self.pub_polygon_marker = rospy.Publisher("polygon_detection/marker", MarkerArray, queue_size=10)
 
         # Detection model
         self.model_path = os.getcwd() + '/GEMstack/knowledge/detection/cone.pt'
@@ -378,7 +379,9 @@ def viz_object_states(self, cone_3d_centers, cone_3d_dims, cv_image, boxes):
         # Create vehicle marker
         ros_vehicle_marker = create_bbox_marker([[0.0, 0.0, 0.0]], [[0.8, 0.5, 0.3]], (0.0, 0.0, 1.0, 1), "vehicle")
         self.pub_vehicle_marker.publish(ros_vehicle_marker)
-        # Delete previous parking spot markers
+        # Delete previous markers
+        ros_delete_polygon_marker = delete_markers("polygon", 1)
+        self.pub_polygon_marker.publish(ros_delete_polygon_marker)
         ros_delete_parking_spot_markers = delete_markers("parking_spot", 1)
         self.pub_parking_spot_marker.publish(ros_delete_parking_spot_markers)
         # Draw 3D cone centers and dimensions
@@ -408,6 +411,9 @@ def detect_parking_spot(self, cone_3d_centers):
         if len(candidates) > 0:
             closest_spot = candidates[0]
             # print(f"-----closest_spot: {closest_spot}")
+            # Draw polygon first
+            ros_polygon_marker = create_polygon_marker(cone_ground_centers_2D, ref_frame="vehicle")
+            self.pub_polygon_marker.publish(ros_polygon_marker)
             # Create parking spot marker
             ros_parking_spot_marker = create_parking_spot_marker(closest_spot, ref_frame="vehicle")
             self.pub_parking_spot_marker.publish(ros_parking_spot_marker)

GEMstack/onboard/perception/parking_utils.py

@@ -42,7 +42,7 @@ def findAllCandidateParkingLot(cornerPts, angleStepDegree=5, positionStrideMeter
 
 def drawCarPose(img, center, angleDegree, color=(0, 0, 255), scale=100):
     rect = (center, (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree))
-    box = cv2.boxPoints(rect)  # 四个角点
+    box = cv2.boxPoints(rect)
     box_scaled = np.int32(box * scale)
 
     cv2.polylines(img, [box_scaled], isClosed=True, color=color, thickness=2)

GEMstack/onboard/perception/visualization_utils.py

@@ -2,6 +2,8 @@
 from visualization_msgs.msg import Marker, MarkerArray
 from geometry_msgs.msg import Point
 import sensor_msgs.point_cloud2 as pc2
+from scipy.spatial import ConvexHull
+import numpy as np
 import cv2
 import rospy
 import struct
@@ -124,6 +126,48 @@ def create_bbox_marker(centroids, dimensions, color = (0.0, 1.0, 1.5, 0.2), ref_
     return marker_array
 
 
+def order_points_convex_hull(points_2d):
+    points_np = np.array(points_2d)
+    hull = ConvexHull(points_np)
+    ordered = [points_np[i] for i in hull.vertices]
+    return ordered
+
+
+def create_polygon_marker(vertices_2d, ref_frame="map"):
+    vertices_2d = order_points_convex_hull(vertices_2d)
+
+    marker_array = MarkerArray()
+
+    marker = Marker()
+    marker.header.frame_id = ref_frame
+    marker.header.stamp = rospy.Time.now()
+    marker.ns = "polygon"
+    marker.id = 0
+    marker.type = Marker.LINE_STRIP
+    marker.action = Marker.ADD
+
+    # Style
+    marker.scale.x = 0.1  # Line width
+
+    # Color (blue)
+    marker.color.r = 0.0
+    marker.color.g = 1.0
+    marker.color.b = 0.0
+    marker.color.a = 1.0
+
+    marker.pose.orientation.w = 1.0
+
+    # Convert 2D vertices into geometry_msgs/Point with z=0.0
+    for vx, vy in vertices_2d:
+        marker.points.append(Point(x=vx, y=vy, z=0.0))
+
+    # Close the loop by repeating the first point
+    marker.points.append(Point(x=vertices_2d[0][0], y=vertices_2d[0][1], z=0.0))
+
+    marker_array.markers.append(marker)
+    return marker_array
+
+
 def create_parking_spot_marker(closest_spot, length = GEM_E4_LENGTH, width = GEM_E4_WIDTH, ref_frame="map"):
     marker_array = MarkerArray()