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

code/mapping/ext_modules/mapping_common/shape.py

@@ -1,13 +1,14 @@
 from dataclasses import dataclass
 from typing import Optional
-
 import rospy
-
+from typing import List, Optional
+from shapely.geometry import Polygon as ShapelyPolygon
 from .transform import Transform2D
 from mapping import msg
-
+from transform import Point2
 from tf.transformations import quaternion_from_euler
 from visualization_msgs.msg import Marker
+from geometry_msgs.msg import Point
 
 
 @dataclass
@@ -155,6 +156,59 @@ def to_marker(self, transform: Transform2D) -> Marker:
         return m
 
 
+@dataclass(init=False)
+class Polygon(Shape2D):
+    """Polygon defined by a list of Point2 objects."""
+
+    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.points) >= 3, "Polygon requires at least 3 points."
+        points = [Point2.from_ros_msg(p) for p in m.points]
+        return Polygon(points=points, offset=Transform2D.from_ros_msg(m.offset))
+
+    def to_ros_msg(self) -> msg.Shape2D:
+        m = super().to_ros_msg()
+        m.points = [pt.to_ros_msg() for pt in self.points]
+        return m
+
+    def to_shapely(self) -> ShapelyPolygon:
+        """Convert to a Shapely Polygon."""
+        coordinates = [(pt.x(), pt.y()) for pt in self.points]
+        return ShapelyPolygon(coordinates)
+
+    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
+
+        # Transform and add points to marker
+        for pt in self.points:
+            if isinstance(pt, Point2):  # Sicherstellen, dass der Punkt korrekt ist
+                transformed_pt: Point2 = (
+                    transform * pt
+                )  # Transform2D.__mul__ auf Point2 anwenden
+                p = Point()
+                p.x = transformed_pt.x()
+                p.y = transformed_pt.y()
+                p.z = 0.0
+                m.points.append(p)
+
+        # Close the polygon loop
+        if len(m.points) > 0:
+            m.points.append(m.points[0])
+
+        return m
+
+
 _shape_supported_classes = [Rectangle, Circle]
 _shape_supported_classes_dict = {}
 for t in _shape_supported_classes:

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,
+            + self.radar_cluster_entities
         )
         msg = map.to_ros_msg()
         self.map_publisher.publish(msg)