From ed2521f0e42991571568cc44051c1823acdcc26e Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 15 Jan 2025 13:15:56 +0100 Subject: [PATCH 1/9] Implemented polygon shape, dummy subscribers for entity lists from radar and lidar nodes and added entities to publishe map object. --- .../ext_modules/mapping_common/shape.py | 60 ++++++++++++++++++- code/mapping/src/mapping_data_integration.py | 26 +++++++- 2 files changed, 82 insertions(+), 4 deletions(-) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 8898bf7f..99078f42 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/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: diff --git a/code/mapping/src/mapping_data_integration.py b/code/mapping/src/mapping_data_integration.py index 82cbcf61..543c70b0 100755 --- a/code/mapping/src/mapping_data_integration.py +++ b/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) From ccc37d41746fe3dd270d1ebee1fcc9f0aeae5008 Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Wed, 15 Jan 2025 13:47:13 +0100 Subject: [PATCH 2/9] Update code/mapping/src/mapping_data_integration.py Fixed case of cluster entities being None. Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- code/mapping/src/mapping_data_integration.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/code/mapping/src/mapping_data_integration.py b/code/mapping/src/mapping_data_integration.py index 543c70b0..fa26aefe 100755 --- a/code/mapping/src/mapping_data_integration.py +++ b/code/mapping/src/mapping_data_integration.py @@ -216,8 +216,8 @@ def publish_new_map(self, timer_event=None): timestamp=stamp, entities=[hero_car] + self.entities_from_lidar_marker() - + self.lidar_cluster_entities, - + self.radar_cluster_entities + + (self.lidar_cluster_entities or []) + + (self.radar_cluster_entities or []) ) msg = map.to_ros_msg() self.map_publisher.publish(msg) From 40633c3f6162ae0f214725ea300e9326275f3bfc Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 15 Jan 2025 13:20:47 +0000 Subject: [PATCH 3/9] Updated version of polygon. Implemented tests for the polygon. --- .../ext_modules/mapping_common/shape.py | 52 ++++++++++--------- code/mapping/src/mapping_data_integration.py | 4 +- .../tests/mapping_common/test_shape.py | 15 +++++- code/perception/src/lidar_distance.py | 26 ++++++++++ 4 files changed, 70 insertions(+), 27 deletions(-) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 99078f42..76c65fd7 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/code/mapping/ext_modules/mapping_common/shape.py @@ -1,11 +1,9 @@ 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 .transform import Transform2D, Point2 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 @@ -160,6 +158,7 @@ def to_marker(self, transform: Transform2D) -> Marker: 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): @@ -170,46 +169,51 @@ def __init__(self, points: List[Point2], offset: Optional[Transform2D] = None): @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] + 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() - m.points = [pt.to_ros_msg() for pt in self.points] + dimensions = [] + for p in self.points: + dimensions.append(p.x()) + dimensions.append(p.y()) + m.dimensions = dimensions 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 + # Initialisiere m.points als leere Liste + m.points = [] + + # Transformiere und füge Punkte hinzu 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 + 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] +_shape_supported_classes = [Rectangle, Circle, Polygon] _shape_supported_classes_dict = {} for t in _shape_supported_classes: t_name = t.__name__.lower() diff --git a/code/mapping/src/mapping_data_integration.py b/code/mapping/src/mapping_data_integration.py index 543c70b0..9e1cb24f 100755 --- a/code/mapping/src/mapping_data_integration.py +++ b/code/mapping/src/mapping_data_integration.py @@ -216,8 +216,8 @@ def publish_new_map(self, timer_event=None): timestamp=stamp, entities=[hero_car] + self.entities_from_lidar_marker() - + self.lidar_cluster_entities, - + self.radar_cluster_entities + + (self.lidar_cluster_entities or []), + + (self.radar_cluster_entities or []) ) msg = map.to_ros_msg() self.map_publisher.publish(msg) diff --git a/code/mapping/tests/mapping_common/test_shape.py b/code/mapping/tests/mapping_common/test_shape.py index 70bda04f..11b4ce4e 100644 --- a/code/mapping/tests/mapping_common/test_shape.py +++ b/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 diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 2c34f832..0f666822 100755 --- a/code/perception/src/lidar_distance.py +++ b/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. From feb7ce113bc6f9a814098ccbf2c9907d1d03895b Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 15 Jan 2025 15:22:14 +0100 Subject: [PATCH 4/9] Fixed linter issues. --- .../ext_modules/mapping_common/shape.py | 1 - code/mapping/src/mapping_data_integration.py | 10 +++---- .../tests/mapping_common/test_shape.py | 1 - code/perception/src/lidar_distance.py | 26 ------------------- 4 files changed, 5 insertions(+), 33 deletions(-) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 76c65fd7..918d70f3 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/code/mapping/ext_modules/mapping_common/shape.py @@ -1,7 +1,6 @@ from dataclasses import dataclass import rospy 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 diff --git a/code/mapping/src/mapping_data_integration.py b/code/mapping/src/mapping_data_integration.py index d9410286..100ae8a7 100755 --- a/code/mapping/src/mapping_data_integration.py +++ b/code/mapping/src/mapping_data_integration.py @@ -95,7 +95,7 @@ def lidar_cluster_entities_callback(self, data: MapMsg): def radar_cluster_entities_callback(self, data: MapMsg): self.radar_cluster_entities = data.entities - + def radar_marker_callback(self, data: MarkerArray): self.radar_marker_data = data @@ -269,10 +269,10 @@ def publish_new_map(self, timer_event=None): map = Map( timestamp=stamp, entities=[hero_car] - + self.entities_from_lidar_marker(), - + self.entities_from_radar_marker(), - + (self.lidar_cluster_entities or []), - + (self.radar_cluster_entities or []) + + self.entities_from_lidar_marker() + + self.entities_from_radar_marker() + + (self.lidar_cluster_entities or []) + + (self.radar_cluster_entities or []), ) msg = map.to_ros_msg() self.map_publisher.publish(msg) diff --git a/code/mapping/tests/mapping_common/test_shape.py b/code/mapping/tests/mapping_common/test_shape.py index 11b4ce4e..b53ab889 100644 --- a/code/mapping/tests/mapping_common/test_shape.py +++ b/code/mapping/tests/mapping_common/test_shape.py @@ -1,6 +1,5 @@ from mapping_common import shape from mapping_common.transform import Transform2D, Vector2, Point2 -import numpy as np def test_rectangle_conversion(): diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 0f666822..2c34f832 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -9,8 +9,6 @@ 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 @@ -392,30 +390,6 @@ 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. From f71ad138f88a1aec4370bfe49dc8d72ca5feacfa Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 15 Jan 2025 16:03:53 +0100 Subject: [PATCH 5/9] Consistent english comments fixed. --- code/mapping/ext_modules/mapping_common/shape.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 918d70f3..2e15de08 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/code/mapping/ext_modules/mapping_common/shape.py @@ -157,7 +157,7 @@ def to_marker(self, transform: Transform2D) -> Marker: class Polygon(Shape2D): """Polygon defined by a list of Point2 objects.""" - # points attribute does not need a redundant point for start and end + # 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): @@ -171,7 +171,7 @@ 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 + # 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) @@ -193,19 +193,19 @@ def to_marker(self, transform: Transform2D) -> Marker: m.type = Marker.LINE_STRIP m.scale.x = 0.05 # Line thickness - # Initialisiere m.points als leere Liste + # Initialize m.points as an empty list m.points = [] - # Transformiere und füge Punkte hinzu + # Transform and add points for pt in self.points: - transformed_pt: Point2 = transform * pt # Transformation anwenden + transformed_pt: Point2 = transform * pt # Apply transformation 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 + # Close the polygon loop if len(m.points) > 0: m.points.append(m.points[0]) From 06f8779d0d5ef85f107d23804de426e39f052f7e Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 15 Jan 2025 16:08:18 +0100 Subject: [PATCH 6/9] Fixed issue with coordinates of polygon points because of double transformation. --- code/mapping/ext_modules/mapping_common/shape.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 2e15de08..00dfb03f 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/code/mapping/ext_modules/mapping_common/shape.py @@ -198,7 +198,7 @@ def to_marker(self, transform: Transform2D) -> Marker: # Transform and add points for pt in self.points: - transformed_pt: Point2 = transform * pt # Apply transformation + transformed_pt: Point2 = pt # Apply transformation p = Point() p.x = transformed_pt.x() p.y = transformed_pt.y() From a45e4cbb532f9d0d0ad2cb6a574f7bc691675868 Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:52:40 +0100 Subject: [PATCH 7/9] Update code/mapping/ext_modules/mapping_common/shape.py Co-authored-by: Zelberor <72208402+Zelberor@users.noreply.github.com> --- code/mapping/ext_modules/mapping_common/shape.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 00dfb03f..7961f3c3 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/code/mapping/ext_modules/mapping_common/shape.py @@ -198,10 +198,9 @@ def to_marker(self, transform: Transform2D) -> Marker: # Transform and add points for pt in self.points: - transformed_pt: Point2 = pt # Apply transformation p = Point() - p.x = transformed_pt.x() - p.y = transformed_pt.y() + p.x = pt.x() + p.y = pt.y() p.z = 0.0 m.points.append(p) From d175c3213f682a8dfe8a562b591b71f640087701 Mon Sep 17 00:00:00 2001 From: michalal7 Date: Wed, 15 Jan 2025 16:53:33 +0100 Subject: [PATCH 8/9] Fixed issue in mapping_data_integration regarding data structure inconsistencies. --- code/mapping/src/mapping_data_integration.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/code/mapping/src/mapping_data_integration.py b/code/mapping/src/mapping_data_integration.py index 100ae8a7..c967b7b6 100755 --- a/code/mapping/src/mapping_data_integration.py +++ b/code/mapping/src/mapping_data_integration.py @@ -31,8 +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 + 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): @@ -91,10 +91,10 @@ 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 + self.lidar_cluster_entities_data = data def radar_cluster_entities_callback(self, data: MapMsg): - self.radar_cluster_entities = data.entities + self.radar_cluster_entities_data = data def radar_marker_callback(self, data: MarkerArray): self.radar_marker_data = data @@ -265,14 +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.lidar_cluster_entities or []) - + (self.radar_cluster_entities or []), + + lidar_cluster_entities + + radar_cluster_entities, ) msg = map.to_ros_msg() self.map_publisher.publish(msg) From cf11cd51b6c6d772e849c3365052593bb0dd0e45 Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:59:33 +0100 Subject: [PATCH 9/9] Update code/mapping/ext_modules/mapping_common/shape.py Co-authored-by: Zelberor <72208402+Zelberor@users.noreply.github.com> --- code/mapping/ext_modules/mapping_common/shape.py | 1 + 1 file changed, 1 insertion(+) diff --git a/code/mapping/ext_modules/mapping_common/shape.py b/code/mapping/ext_modules/mapping_common/shape.py index 7961f3c3..18bf323e 100644 --- a/code/mapping/ext_modules/mapping_common/shape.py +++ b/code/mapping/ext_modules/mapping_common/shape.py @@ -161,6 +161,7 @@ class Polygon(Shape2D): 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)