feature-shape2d-polygon-class-for-intermediate-layer-and-cluster-integration

code/mapping/ext_modules/mapping_common/shape.py

@@ -1,13 +1,12 @@
 from dataclasses import dataclass
-from typing import Optional
-
 import rospy
-
-from .transform import Transform2D
+from typing import List, Optional
+from shapely.geometry import Polygon as ShapelyPolygon
+from .transform import Transform2D, Point2
 from mapping import msg
-
 from tf.transformations import quaternion_from_euler
 from visualization_msgs.msg import Marker
+from geometry_msgs.msg import Point
 
 
 @dataclass
@@ -155,7 +154,66 @@ def to_marker(self, transform: Transform2D) -> Marker:
         return m
 
 
-_shape_supported_classes = [Rectangle, Circle]
+@dataclass(init=False)
+class Polygon(Shape2D):
+    """Polygon defined by a list of Point2 objects."""
+
+    # points attribute does not need a redundant point for start and end
+    points: List[Point2]
+
+    def __init__(self, points: List[Point2], offset: Optional[Transform2D] = None):
+        if offset is None:
+            offset = Transform2D.identity()
+        super().__init__(offset=offset)
+        self.points = points
+
+    @staticmethod
+    def _from_ros_msg(m: msg.Shape2D) -> "Shape2D":
+        assert len(m.dimensions) >= 6 and (
+            len(m.dimensions) % 2 == 0
+        ), "Polygon requires at least 3 points."
+        # Konvertiere die flache Liste in Point2-Objekte
+        points = [
+            Point2.new(m.dimensions[i], m.dimensions[i + 1])
+            for i in range(0, len(m.dimensions), 2)
+        ]
+        return Polygon(points=points, offset=Transform2D.from_ros_msg(m.offset))
+
+    def to_ros_msg(self) -> msg.Shape2D:
+        m = super().to_ros_msg()
+        dimensions = []
+        for p in self.points:
+            dimensions.append(p.x())
+            dimensions.append(p.y())
+        m.dimensions = dimensions
+        return m
+
+    def to_marker(self, transform: Transform2D) -> Marker:
+        """Convert to a visualization Marker for RViz."""
+        m = super().to_marker(transform)
+        m.type = Marker.LINE_STRIP
+        m.scale.x = 0.05  # Line thickness
+
+        # Initialisiere m.points als leere Liste
+        m.points = []
+
+        # Transformiere und füge Punkte hinzu
+        for pt in self.points:
+            transformed_pt: Point2 = transform * pt  # Transformation anwenden
+            p = Point()
+            p.x = transformed_pt.x()
+            p.y = transformed_pt.y()
+            p.z = 0.0
+            m.points.append(p)
+
+        # Schließe die Polygon-Schleife
+        if len(m.points) > 0:
+            m.points.append(m.points[0])
+
+        return m
+
+
+_shape_supported_classes = [Rectangle, Circle, Polygon]
 _shape_supported_classes_dict = {}
 for t in _shape_supported_classes:
     t_name = t.__name__.lower()

code/mapping/src/mapping_data_integration.py

@@ -31,6 +31,8 @@ class MappingDataIntegrationNode(CompatibleNode):
     lidar_data: Optional[PointCloud2] = None
     hero_speed: Optional[CarlaSpeedometer] = None
     lidar_marker_data: Optional[MarkerArray] = None
+    lidar_cluster_entities: Optional[List[Entity]] = None
+    radar_cluster_entities: Optional[List[Entity]] = None
 
     def __init__(self, name, **kwargs):
         super().__init__(name, **kwargs)
@@ -53,6 +55,18 @@ def __init__(self, name, **kwargs):
             callback=self.lidar_marker_callback,
             qos_profile=1,
         )
+        self.new_subscription(
+            topic=self.get_param("~entity_topic", "/paf/hero/Lidar/cluster_entities"),
+            msg_type=MapMsg,
+            callback=self.lidar_cluster_entities_callback,
+            qos_profile=1,
+        )
+        self.new_subscription(
+            topic=self.get_param("~entity_topic", "/paf/hero/Radar/cluster_entities"),
+            msg_type=MapMsg,
+            callback=self.radar_cluster_entities_callback,
+            qos_profile=1,
+        )
 
         self.map_publisher = self.new_publisher(
             msg_type=MapMsg,
@@ -68,6 +82,12 @@ def hero_speed_callback(self, data: CarlaSpeedometer):
     def lidar_marker_callback(self, data: MarkerArray):
         self.lidar_marker_data = data
 
+    def lidar_cluster_entities_callback(self, data: MapMsg):
+        self.lidar_cluster_entities = data.entities
+
+    def radar_cluster_entities_callback(self, data: MapMsg):
+        self.radar_cluster_entities = data.entities
+
     def lidar_callback(self, data: PointCloud2):
         self.lidar_data = data
 
@@ -193,7 +213,11 @@ def publish_new_map(self, timer_event=None):
 
         stamp = rospy.get_rostime()
         map = Map(
-            timestamp=stamp, entities=[hero_car] + self.entities_from_lidar_marker()
+            timestamp=stamp,
+            entities=[hero_car]
+            + self.entities_from_lidar_marker()
+            + (self.lidar_cluster_entities or [])
+            + (self.radar_cluster_entities or []),
         )
         msg = map.to_ros_msg()
         self.map_publisher.publish(msg)

code/mapping/tests/mapping_common/test_shape.py

@@ -1,5 +1,6 @@
 from mapping_common import shape
-from mapping_common.transform import Transform2D, Vector2
+from mapping_common.transform import Transform2D, Vector2, Point2
+import numpy as np
 
 
 def test_rectangle_conversion():
@@ -15,3 +16,15 @@ def test_circle_conversion():
     msg = s.to_ros_msg()
     s_conv = shape.Shape2D.from_ros_msg(msg)
     assert s == s_conv
+
+
+def test_polygon_conversion():
+    data = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]
+    p_list = []
+    for i in range(0, 6, 2):
+        p_list.append(Point2.new(data[i], data[i + 1]))
+
+    polygon = shape.Polygon(p_list)
+    msg = polygon.to_ros_msg()
+    polygon_conv = shape.Shape2D.from_ros_msg(msg)
+    assert polygon == polygon_conv

code/perception/src/lidar_distance.py

@@ -9,6 +9,8 @@
 from cv_bridge import CvBridge
 from tf.transformations import quaternion_from_matrix
 from visualization_msgs.msg import Marker, MarkerArray
+from shapely.geometry import Polygon as ShapelyPolygon
+from mapping.ext_modules.mapping_common.shape import Polygon
 
 # from mpl_toolkits.mplot3d import Axes3D
 # from itertools import combinations
@@ -390,6 +392,30 @@ def generate_bounding_boxes(points_with_labels):
     return bounding_boxes
 
 
+def create_polygons_and_entities(label, bbox):
+    """
+    Erstelle ein Shapely-Polygon, ein Shape2D-Polygon und ein Entity aus einer Bounding Box.
+
+    Args:
+        bbox (tuple): Bounding Box als (x_min, x_max, y_min, y_max).
+        entity_id (str): ID der zu erstellenden Entity.
+        frame_id (str): Referenzrahmen für die Pose. Default ist "map".
+
+    Returns:
+        tuple: (ShapelyPolygon, Shape2DPolygon, Entity)
+    """
+    # Schritt 1: Shapely-Polygon aus Bounding Box
+    x_min, x_max, y_min, y_max = bbox
+    coordinates = [
+        (x_min, y_min),
+        (x_max, y_min),
+        (x_max, y_max),
+        (x_min, y_max),
+        (x_min, y_min),  # Schließe das Polygon
+    ]
+    shapely_polygon = ShapelyPolygon(coordinates)
+
+
 def create_bounding_box_marker(label, bbox, bbox_type="aabb"):
     """
     Creates an RViz Marker for visualizing a 3D bounding box using Marker.CUBE.