Merge pull request #616 from una-auxme/615-feature-shape2d-polygon-cl…

…ass-for-intermediate-layer-and-cluster-integration

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

code/mapping/ext_modules/mapping_common/shape.py

@@ -1,13 +1,11 @@
 from dataclasses import dataclass
-from typing import Optional
-
 import rospy
-
-from .transform import Transform2D
+from typing import List, Optional
+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 +153,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."""
+
+    # The 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):
+        assert len(points) >= 3, "Polygon requires at least 3 points."
+        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."
+        # Convert the flat list into Point2 objects
+        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
+
+        # Initialize m.points as an empty list
+        m.points = []
+
+        # Transform and add points
+        for pt in self.points:
+            p = Point()
+            p.x = pt.x()
+            p.y = 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, 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_data: Optional[List[Entity]] = None
+    radar_cluster_entities_data: Optional[List[Entity]] = None
     radar_marker_data: Optional[MarkerArray] = None
 
     def __init__(self, name, **kwargs):
@@ -54,6 +56,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.new_subscription(
             topic=self.get_param("~marker_topic", "/paf/hero/Radar/Marker"),
@@ -76,6 +90,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 = data
+
+    def radar_cluster_entities_callback(self, data: MapMsg):
+        self.radar_cluster_entities_data = data
+
     def radar_marker_callback(self, data: MarkerArray):
         self.radar_marker_data = data
 
@@ -245,12 +265,21 @@ def publish_new_map(self, timer_event=None):
         ):
             return
 
+        lidar_cluster_entities = (
+            Map.from_ros_msg(self.lidar_cluster_entities_data).entities or []
+        )
+        radar_cluster_entities = (
+            Map.from_ros_msg(self.radar_cluster_entities_data).entities or []
+        )
+
         stamp = rospy.get_rostime()
         map = Map(
             timestamp=stamp,
             entities=[hero_car]
             + self.entities_from_lidar_marker()
-            + self.entities_from_radar_marker(),
+            + self.entities_from_radar_marker()
+            + lidar_cluster_entities
+            + radar_cluster_entities,
         )
         msg = map.to_ros_msg()
         self.map_publisher.publish(msg)

code/mapping/tests/mapping_common/test_shape.py

@@ -1,5 +1,5 @@
 from mapping_common import shape
-from mapping_common.transform import Transform2D, Vector2
+from mapping_common.transform import Transform2D, Vector2, Point2
 
 
 def test_rectangle_conversion():
@@ -15,3 +15,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